From f87c081475006fdb2a2238be9b79a4e5f9678523 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 21 Aug 2022 22:06:53 +0300 Subject: [PATCH 001/199] gps fix estimation squashed commit --- docs/GPS_fix_estimation.md | 87 +++++++++++++ docs/Screenshots/gps_off_box.png | Bin 0 -> 10195 bytes src/main/fc/fc_msp_box.c | 6 +- src/main/fc/rc_modes.h | 1 + src/main/fc/runtime_config.h | 1 + src/main/fc/settings.yaml | 5 + src/main/flight/failsafe.c | 2 +- src/main/flight/wind_estimator.c | 3 +- src/main/io/gps.c | 123 ++++++++++++++++++ src/main/io/osd.c | 22 ++-- src/main/io/osd_dji_hd.c | 2 +- src/main/navigation/navigation.c | 13 +- src/main/navigation/navigation.h | 2 + src/main/navigation/navigation_fixedwing.c | 2 +- .../navigation/navigation_pos_estimator.c | 13 +- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/frsky_d.c | 6 +- src/main/telemetry/ghst.c | 2 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/sim.c | 2 +- src/main/telemetry/smartport.c | 2 +- src/main/telemetry/srxl.c | 4 +- 22 files changed, 273 insertions(+), 29 deletions(-) create mode 100644 docs/GPS_fix_estimation.md create mode 100644 docs/Screenshots/gps_off_box.png diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md new file mode 100644 index 00000000000..4cf9f010837 --- /dev/null +++ b/docs/GPS_fix_estimation.md @@ -0,0 +1,87 @@ +# GPS Fix estimation (dead reconing, RTH without GPS) for fixed wing + +Video demonstration + +[![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/wzvgRpXCS4U/0.jpg)](https://www.youtube.com/watch?v=wzvgRpXCS4U) + +There is possibility to allow plane to estimate it's position when GPS fix is lost. +The main purpose is RTH without GPS. +It works on fixed wing only. + +Plane should have the following sensors: +- acceleromenter, gyroscope +- barometer +- GPS +- magnethometer +- pitot (optional) + +By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above inreachable spaces, plane will be lost. + +GPS fix estimation allows to recover plane using magnetometer and baromener only. + +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. + +# How it works ? + +In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints. + + +Without GPS fix, plane has nose heading from magnetometer only. + +To navigate without GPS fix, we make the following assumptions: +- plane is flying in the direction where nose is pointing +- plane is flying with constant speed, specified in settings + +It is posible to roughtly estimate position using these assumptions. To increase heading accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase speed estimation accuracy, plane will use pitot tube (if available). + +From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. + +It is assumed, that plane will fly in roughtly estimated direction to home position untill either GPS fix or RC signal is recovered. + +*Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. + +# Settings + +GPS Fix estimation is enabled with CLI command: + +```set inav_allow_gps_fix_estimation=ON``` + +Also you have to specify criuse airspeed of the plane. + +The get cruise airspeed, make test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. + +Cruise airspeed is specified in m/s. + +To convert km/h to m/s, multiply by 27.77. + + +Example: 100 km/h = 100 * 27.77 = 2777 m/s + +```set fw_reference_airspeed=2777``` + +*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Icrease cruise speed with throttle" - do not use it without GPS Fix.* + +*If pitot is available, pitot sensor data will be used instead of constant.* + +*Note related command: to continue mission without RC signal, see command ```set failsafe_mission=OFF```.* + +**After entering CLI command, make sure that settings are saved:** + +```save``` + +# Disabling GPS sensor from RC controller + +![](Screenshots/gps_off_box.png) + +For testing purpoces, it is now possible to disable GPS sensor from RC controller: + +*GPS can be disabled only after 1) initial GPS fix is acquired 2) in ARMED mode.* + +# Is it possible to implement this for multirotor ? + +There are some ideas, but there is no solution now. We can not make assumptions with multirotor which we can make with a fixed wing. + + +# Links + +INAV HITL https://github.com/RomanLut/INAV-X-Plane-HITL diff --git a/docs/Screenshots/gps_off_box.png b/docs/Screenshots/gps_off_box.png new file mode 100644 index 0000000000000000000000000000000000000000..1c912492d2eed99c27a623a2dcaa4735d4ed4f0e GIT binary patch literal 10195 zcmaKS2Q-{t6SowS5F|nb5rXKwtg=D$-d68XmMFW6U6vIs5;ce(J%}DekFtnf62j`e zx7DIX|5lR!|NYK+zxTVx;XcoqJ2Q7??lbcnH&|0$ftZk*5DyQJSV>V9gok&10ynq0 zc@_6N?K6!6?hh40&H$kevq2y&T)=oz)-X#jy%NO277PMgSbI2kf}i5yU1PA*F+doo zsft;_Alw$0G~DhGC){d0yr+`xP8L=UU|jv(OO6(nFjs^)15VRFTYxzI%N7d%YbKn*c-$?Vc=)(^FSqm$ zp_8YTg!1$NY_wm5wq^Y6d=*Y;6Q5k2pv&^vjy4{~f%Z?LB zvbQd|-e$?{TUoeu^}|&9)ION^0STRiBWfjC3ejOakx1k|_6Y%==XM?>!I`7Z2IbmU8)L{XD#MtlY>FIy0p6p<)N<&La`{>c5 zD_5?VY_&A_ky>!YJFM6m8`Fs6&w+5P(x0U>kALXEkcF0}Ig*Adj(WPZ5Ab?X;!M%# z%qg7e>gINQd^|Ssu8jt7b7_2m)fq*YG`@_dxuv;9@aD-i5 z2%!jVjk{8^SVIh4hX#I*O9!ZoKIb)vU@+(kQoOR?W3k@@nwN{n8|}ibS!a z&bhe|F)c*DzmoIa#~`}&qD)f^$n?O}yD!U4#j^js2Puns1Qi#Bc2CV%?EZ2%u~aeO@T_MJuPnPz5%M4`dkC9gdu6W5~9H|q68%r!Ih3QV~fp6tEK%*<3#R(^OV z1|*xW!=99sWNd7_>Vbdt%9uIsWHnioEY`9rT3at{Y-wO6vDOff0bDgpz=a^(b^#U8 z^H5s*ff7Y=l|9u_vR22ugi>|ymHqHRA(c5ORU$!B3G7W?etxNJI^Zw{B=RH8;Q1GS zP;tk{H=$81if_i;vP#YDW?t8Wf+OBx*6Lcwf{P=Hs?A0h*@y`f1cACul&6gmD#>IF zj*w3U80S<8kOp7QiJg1_lZ|& z(A&nH-5e6l!YyHJ@60cwOiM)2H`IK%6*w-uvAy^CN8;4f46UBM-k__#cbh@+zn+G_ z0$u5q+V80;C3Ql57NC*zerapVVr(*OGs)Opxk#jvBE}l4=0VS`%CGoEMG(A|V^kh@uIMj6w(^~Dx9^*wult50SayM^hA|359J*?%v zN{62L*$Quy)kq>Qwjr0*2zPj~MuR702t60$1=DKaC}-rc|d1D}=jN9h!4Gw-?Vpgp~u=%V== zd0}Q&o3_)y+KBsl?5;x%#J8jHRYVGF4lF5%fYn1BwgqcBQ6 zsLE+hKv5c|?1XE|WHTqw?QlFG-^uPl(Afq}sVhHB`84%5&5`|tp6wIumwC6+ACwd!Dl#QL4@rm; z%Ed9CiC3`;R$Nr;zT@a0I4vX4rmEWYUc^CN_b8Y#(8qE8mYO(IJ;URiE&B$GThCl$ zTyp%PLs}Vj#RH!N0b@C5j?5-+(G zWvH?LPd1G8tD21jD-YNx5fB#N68^1PvXop^%fUvqd6NaqZS>@|^kW8GwsT6y8{BL} z&Nyzt1#>xDg;iaCFNLF|J~=R9wbYJ@d^7O$*~6p z#}gxbW_v7s7Jg*uQq!ZOqXM&)h39jNDB|UNqT)j0qI!T35^q5dP|)WBL+?E04Jdxj zms7?nPMZlxxVumvN~@&0cnsJ^-hO`IlDp~~HgCUWxwfUkgBJ8UMi5G|-O)`d35s!( z*!v-qtB=zZ5ATOYbKNo2*0poivh!8Ts+Hv#fmK&lZY}Bri&_wdV4!$kkL( z<~KwjI}hB>XNxUXIZGk@VfwG=)~xbCxsT=1rXqi;EhoPZ1A>%?Cq6IrJY3e!qNTmQ z@yJi-)tQ*-VJHNpP<5swZdpe;)wNu<^`n@g_YcLD9YYj>i3=<~pBn6JUD@?N2G|RfS>7}fXS3OEx><}2zX&(`HshN9 zAqGD>EJuY}W4cbmBYk1}jf6s_PTEsFuRz|zwS;;7-jQ*S-J;61Qs9%tZ)B8polowQ zH^lwYzU^_!WyXN~_GQXq8fzJ>Vn=iLPfhy8QIqiSc~9ARTwsqWUs1a4m8rQZGIP^P z^emJCgbAsIPmiB}V#9AeICRukU-L2P0%$V-_MPts#KJrA3XBN!8c$4Bl(=`<$QRF^ z$tRJ&jG8Mc(#6^8oWK1VLz`H;MmY`awMwGUnyf9VxA5?Mlc-J{@!pp2P6zGMNwM9I zZ5e*sHoU%W2J!8|SWNT6*BG0Fb|*z)l#! zZhUnT!Qu*o#dU(Lr;N5UxGZSvv=ASm32y&DF2(ki-m=7iJvM(ZyZHR)-rmCQk-G-2 z47=5=g{yo3>8tc4W)T0WP4%9}%q!KR59Kro83kE|&OXjQPOE$po{Pj)$MPSvGUZ*} z67?*hQ0M_J7u>x5n}?l@_Q7mkTAKN^)JEaZ5-lx*)d?4koA%}j$9xzd(@%hUh4Sfv z%G(r+7CtTsHHrvZ!NI4&*Vbt1SzTqwEnSgF?N5s?#mQ!sR&z4;%586{81IMrzk2n9 z8)q6!K&IomgLnvdJg!^l@a1Y-%}JU1h2(rzo!NWUo4LsFJmkZrO$Jt1Nl;#Sem=XH z*jU9JC`54;478zQ@?X`XQfH2yrN;SIWOrtS-(_$Wfh(qmnir*1Cr|-S*49kM`0v6Z z_>>6-fHww!dc&HNcVcQ{fBg6n5D*X*MTR!plIBU8lfiLp1um_!a)SljaiAyJ!|B^G z=YeznzAFv3dDD&40e;qw3YzHa`zl;^J4Wk{FG_e|pM-)~DsQ|kizIXt7a`ACM<%M| z{WA8QM)^DAYKP^tkHg<`GApsx%72Y`P78Dspudpt2{ z6m47nouFMXxuW*Eici4gkQI~&+s9i^M(}?Lc`0cinOc)l|S)#A2Om_>V6dmq$vhV6isSkNJgt zUG+5G+{FffE_*9l$D%&xFWU!d?}kQHUE2Khuia`I0L_fMSG-pBptv0X=otV-ihbJ3 zXMaF-?Zea`IF}Qoc%e1iT4{I9d38HHLbM>k8^EZa8Vk&}I_wzFA^nf>1Ei$<4*;YV z1|u&^T0Y#a<+?@EI?VeUQ4-@%XybOt%Yax3tMa(n+_cnIcK$Zv1b(}o`Yzr+i4>b} z5`3iV3gcts<{5$&7(?q4_zHV*$Ky^KR5PHJ75Ef~ezuuKGM*pfBL8yGv%aa-Oo)!F zTxl*%f)dzw8K=cndN*{H7qWMZX?~~~*V)gHx-dh(Fiq`ZmvRU_H?osZ)!I8xe6qLQ zxnIk@SASiu?*>oeL7_V&zU~_S9}s@_X0^7+P99|s)=B_yOAXLaJmUG$^S*zVmHr8~ zqE_{@Og)!Fw41lRBV;ReX0K`o<4N~|L(q5N2v13~QGJuUFwk3zBNMHk3wcy4x&9$DkURB-GvT~N=0`{jd z?^5Y2ktRR03pcujy<*cE=3}>ug%=0m$@#26cxxq*Fdg&Fo22)g3kj>yg5(#cA;yX9 z0+4+Bu4cwpo(F*nZMtT5EK){>v^OWN$?>H97A(fQo;`oHb zP!4=NCkeBwQ;k#Pf9h^o|Y!fm%iOk&`k@0jkZLj9IU>{6D(PfjREtj>bt!I12hFDCqmf5>KhN-T;N9nT; z8@>B(o4(3T!*Hi8dl0#M&aHCB>Wg#R+|VYwh8ft%OBkmAVy*p3t|1|?ebx^z9_iRJc>$L&w9R36?0VR zliYtu{b}H`o24%G+x>sLS+{4!vn&qZR8Ip};u0>~On7(?xMd#1)U>xuM!x&o=@P#E zPp>SQgB6XcIA6_4J738pB_{Bip02FSH-Gu_LPsZNe7(23yW1OBTTp;m>M*oPNJvQY zIj`P_;f!-Rr1EGfC?pk`qViSZscDLdpH_TNx^S6m2}VwN*j}8U;o}EtCI2BUYE4Hf zx*-rTadDhxbzd?v=GLpraP6aV%nQLs;;6m(2uUbx*xK3};dWrM?PzCrw3{Y*e9-9k z`q!`RZR4W3?)jfh&F$*%Mbhi*Qy&3@Ip8iXJERHsJI-3~X`HU&nW;9mXNHCRJY% z6eMWS*bk?Q{o{Qq0h@0xpD*QYY1q|PES1gnFManp+JIe%3kxH^jg&0&n0!bf@IMdb zspqAxKJ)NsI62!YC@6r%GPjls2~|+?n0)a$KHJk|h~zY_;e<`sZ;V%_iRtPY=yZ?( z-FbbMzJ0-=XY-5hV&wWrx(@rQM-^l|F*a5%*1pdwbg0mCvF}ZUnM2Ki`Bp;%Vzr+! zkQ8T*SZO`Nk0}K$>dPOqxQ=H$3-=`k&-d-@u)B!_aiwP3)xw@?HbzEcQ9@gkY@C>a ze{;)Pj!eZ)3Rd6xV-~pRXeKw~kXNqn#3AlSsHe+W5z$zBhuFSZdjwt15|4mD^jOP1 z7In>}4!3YxV4PdO)5r4in2OjvV=-n%Mt>t!dxhkN=*qX0hW8AOjcc7|rR@oOlSP}o z<*Gl_X@+xpAgdi~usKX|5&bK(St{|lGb@fC)kC{B^?v)YowN)gmh#=3a3L2kse9?b zJ|)HCgq}`S0YElla>6hmAP&`TB;7@=E66TYOI2cg&%zxDWXESGI>cfoZ^d?0!^Ur`gYe?6y+jz+q6kDoU7Hp9pr>AVkytSfV*N0BqHd5;E=$;_Ar_a2n1qTA|j}>gvHWG z08Fy%)2CB`o_dYT?QL!Sp$Ve9SF#eyVhtyn1_b25dKPIJyyi5*9*wK{UA^v~I;IlR zE;fd2>ng3c1Qe2JcOckOc1Dm@p>ggOeCYgr`D(%GDXl|ugK1S^*D1q43H<0p;yht( zBTTN)KkX;9?B{$t#?8RseSG|Ig~RZOdV*@3TmGQEJ-Hxtm?rB(aa6e?6CojChV(~* z0lTCVnI)o=A-d0|4c;l1Fgm(495m1ZP!MVx{-9FvNPnnRzw&r-@=fV#k=FOG{x>o+ zGIF0-c; zlEV8Yv~*-3rqGrt0ztXOlgv7N1#HMF&}oUYr3=GC#uXPuD? zhm%lk*wt_Hka_%&9OI?FjY4y`n6$NYcaK98lY}o88^2zgZPetbcK3TKj-jm>?Xp7glrpi}Alf3a zFW&nk)n^F*=s2FYkvB!u>4TG!G)JG;^Rp8wnw9-bu*(1xT>h-Jx(`ry5W_4$GS0fR9I8{p?)6d3nhp;0b! zac(Pq?Q>=r8FEj}%SP?TP_J@@i9=tzPlRDKmDO=UN`qmH>Bkz~?{!1(?m+7k^zPhw z#5`w2q38EWS&N2p%2QW^{Lwv>Vfw6&(Bkafuj0UF#(DN)lJh`|PbIbPKN)Q67vBp( z25ugt_%8QvTyCqS z#%^ZxJ9+(qMXK6h73lb95=8LvI; z&r*qax)T%;5plA=y7`)&GAZ^)uVv|YkBCpP_&YzUFV8gX1*zGthlNltFAu}zco-Nm z&esNOYHHrEtSs3qR+rhb?Qn7c<(6NsVkqg-l%knq<6@@1^r<(TZH|w@YnQL%W39vKGMJ(^^~z8x3ht~ zEn<00)>w8nGfN#v*_xDlc85sqah92x3+LmrvOL>df|3`1t0{8=au_ z9&~*UtN6Nc3vXSnf@Z;BG>b-kVB*T@i#tcZ<|8mcY5Wuv6mp_jvoYff;hGJ-0a1?3 zj|acjQrtH^`#jY8y78ipTr zG~PC$XT{|U+?Ol?0HIFfS-DkUUE0ynagRm0JuDP;FuFYDxup?G$&;<<0?k-G&=Ok@ zW=Wm-`Q^>(>Q>!u&%nsYIMe1bHa@kU7v^Jz-N&lLbDr&Vi`x<1e<)${E-KL_sbS~v zFg~2R!f};9#Evfc;`zb&Xkp>niS(A|YF7Ng<|K_I&>PoZt*)*{p}$gff0sO$Dz0ln zg#$fvnS?~;pYL<%j#hpe%wDavvuhYH94vi5#=(7-UOJK8x*EMzKJr`v+++MCuUJwb zE3JTRO|dwiov*S5F?!T$hziWpi8**|j!>gq^A)?D1HL zB8sc~Th#crKIL(T5*bU#(t+jVpoqP^K@rUSRvDi__O0q}50sw!aS1)k3s&#hX(iYzk84EeD5>O8}TR7TXW@}1+%w=VAS_ zhe-4Z#!Pj)lrGZypi3VaqNrxMlnpy&fc;!RB-Ar+6-32qG$;b^Jz5yrWK;15sAsTs zp44*gXZlL@@+Z|Bo0z;@8(=#CBR&}?3fPhmGq9{K3+jB8a09TlcmX1z2a?9+%)!@@Lr&Y%qIzSPqU@*>gVYDh?|94UJ1&S+c z-z62gv?MGieXeYRxG3=O<4}zkkU^IZfX0RE@sR8T`2l}p<<|l}SD1~ZM{%emUf$w? zt39q>{d8p()sSDFc3J#P7NQ`+niaM$lNghZIG(7AS8LvjVX1YQNvZ zDwEKp^rw@=YY=ddszEqQ;9Sb_2ULHdl9>9l7}sX{-O|Lv`}Y>izgJiOxdnswU$ 0 ADD_ACTIVE_BOX(BOXOSDALT1); @@ -400,6 +403,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)), BOXPLANWPMISSION); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSOARING)), BOXSOARING); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGPSOFF)), BOXGPSOFF); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index e72e18a84e6..d8819ad75c1 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -75,6 +75,7 @@ typedef enum { BOXPLANWPMISSION = 46, BOXSOARING = 47, BOXUSER3 = 48, + BOXGPSOFF = 49, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 3a04e192f74..01fc555c42b 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -122,6 +122,7 @@ typedef enum { NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available COMPASS_CALIBRATED = (1 << 8), ACCELEROMETER_CALIBRATED = (1 << 9), + GPS_ESTIMATED_FIX = (1 << 10), NAV_CRUISE_BRAKING = (1 << 11), NAV_CRUISE_BRAKING_BOOST = (1 << 12), NAV_CRUISE_BRAKING_LOCKED = (1 << 13), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f264763e688..be17f963250 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2280,6 +2280,11 @@ groups: default_value: OFF field: allow_dead_reckoning type: bool + - name: inav_allow_gps_fix_estimation + description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS." + default_value: OFF + field: allow_gps_fix_estimation + type: bool - name: inav_reset_altitude description: "Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming)" default_value: "FIRST_ARM" diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 0e761f3bd08..9a1cf2e1944 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -343,7 +343,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set if (failsafeConfig()->failsafe_min_distance > 0 && - sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { + sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { // get the distance to the original arming point uint32_t distance = calculateDistanceToDestination(&original_rth_home); diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 7fe49a45949..c58aeb99a94 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -82,7 +82,8 @@ void updateWindEstimator(timeUs_t currentTimeUs) if (!STATE(FIXED_WING_LEGACY) || !isGPSHeadingValid() || !gpsSol.flags.validVelNE || - !gpsSol.flags.validVelD) { + !gpsSol.flags.validVelD || + STATE(GPS_ESTIMATED_FIX)){ return; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index e05260ff7e0..23fd9254a8e 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -45,14 +45,19 @@ #include "fc/runtime_config.h" #endif +#include "fc/rc_modes.h" + #include "sensors/sensors.h" #include "sensors/compass.h" +#include "sensors/barometer.h" +#include "sensors/pitotmeter.h" #include "io/serial.h" #include "io/gps.h" #include "io/gps_private.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "config/feature.h" @@ -60,6 +65,11 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "flight/imu.h" +#include "flight/wind_estimator.h" +#include "flight/pid.h" +#include "flight/mixer.h" + typedef struct { bool isDriverBased; portMode_t portMode; // Port mode RX/TX (only for serial based) @@ -137,6 +147,117 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) gpsState.timeoutMs = timeoutMs; } + +bool canEstimateGPSFix(void) +{ + return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_GPS) && sensors(SENSOR_BARO) && sensors(SENSOR_MAG) && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); +} + +void updateEstimatedGPSFix(void) { + + static uint32_t lastUpdateMs = 0; + static int32_t estimated_lat = 0; + static int32_t estimated_lon = 0; + static int32_t estimated_alt = 0; + + uint32_t t = millis(); + int32_t dt = t - lastUpdateMs; + lastUpdateMs = t; + + static int32_t last_lat = 0; + static int32_t last_lon = 0; + static int32_t last_alt = 0; + + if (IS_RC_MODE_ACTIVE(BOXGPSOFF)) + { + gpsSol.fixType = GPS_NO_FIX; + gpsSol.hdop = 9999; + gpsSol.numSat = 0; + + //freeze coordinates + gpsSol.llh.lat = last_lat; + gpsSol.llh.lon = last_lon; + gpsSol.llh.alt = last_alt; + + DISABLE_STATE(GPS_FIX); + } + else + { + last_lat = gpsSol.llh.lat; + last_lon = gpsSol.llh.lon; + last_alt = gpsSol.llh.alt; + } + + if (STATE(GPS_FIX) || !canEstimateGPSFix()) { + DISABLE_STATE(GPS_ESTIMATED_FIX); + estimated_lat = gpsSol.llh.lat; + estimated_lon = gpsSol.llh.lon; + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + return; + } + + ENABLE_STATE(GPS_ESTIMATED_FIX); + + gpsSol.fixType = GPS_FIX_3D; + gpsSol.hdop = 99; + gpsSol.flags.hasNewData = true; + gpsSol.numSat = 99; + + gpsSol.eph = 100; + gpsSol.epv = 100; + + gpsSol.flags.validVelNE = 1; + gpsSol.flags.validVelD = 0; //do not provide velocity.z + gpsSol.flags.validEPE = 1; + + float speed = pidProfile()->fixedWingReferenceAirspeed; + +#ifdef USE_PITOT + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) + { + speed = pitotCalculateAirSpeed(); + } +#endif + + float velX = rMat[0][0] * speed; + float velY = -rMat[1][0] * speed; + // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame + + if (isEstimatedWindSpeedValid()) { + velX += getEstimatedWindSpeed(X); + velY += getEstimatedWindSpeed(Y); + } + // here (velX, velY) is estimated horizontal speed with wind influence = ground speed + + if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent() == 0))) + { + velX = 0; + velY = 0; + } + + estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) ); + estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) ); + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + + gpsSol.llh.lat = estimated_lat; + gpsSol.llh.lon = estimated_lon; + gpsSol.llh.alt = estimated_alt; + + gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY); + + float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction + if (groundCourse < 0) { + groundCourse += 2 * M_PIf; + } + gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse); + + gpsSol.velNED[X] = (int16_t)(velX); + gpsSol.velNED[Y] = (int16_t)(velY); + gpsSol.velNED[Z] = 0; +} + + + void gpsProcessNewSolutionData(void) { // Set GPS fix flag only if we have 3D fix @@ -154,6 +275,8 @@ void gpsProcessNewSolutionData(void) // Set sensor as ready and available sensorsSet(SENSOR_GPS); + updateEstimatedGPSFix(); + // Pass on GPS update to NAV and IMU onNewGPSData(); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7198262a229..e53668bad38 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1227,7 +1227,7 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen } } - if (STATE(GPS_FIX)) { + if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); float poiAngle = DEGREES_TO_RADIANS(directionToPoi); @@ -1341,7 +1341,7 @@ static void osdDisplayTelemetry(void) static uint16_t trk_bearing = 0; if (ARMING_FLAG(ARMED)) { - if (STATE(GPS_FIX)){ + if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)){ if (GPS_distanceToHome > 5) { trk_bearing = GPS_directionToHome; trk_bearing += 360 + 180; @@ -1662,8 +1662,10 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_SAT_L; buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); - if (!STATE(GPS_FIX)) { - if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + if (STATE(GPS_ESTIMATED_FIX)) { + if (!STATE(GPS_FIX)) { + strcpy(buff + 2, "ES"); + } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { strcpy(buff + 2, "X!"); } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1717,7 +1719,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); } @@ -2178,11 +2180,11 @@ static bool osdDrawSingleElement(uint8_t item) osdCrosshairPosition(&elemPosX, &elemPosY); osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); - if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if (osdConfig()->hud_homing && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { osdHudDrawHoming(elemPosX, elemPosY); } - if (STATE(GPS_FIX) && isImuHeadingValid()) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && isImuHeadingValid()) { if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { osdHudClear(); @@ -2721,7 +2723,7 @@ static bool osdDrawSingleElement(uint8_t item) bool moreThanAh = false; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); @@ -2792,7 +2794,7 @@ static bool osdDrawSingleElement(uint8_t item) int32_t value = 0; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))&& gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); @@ -2953,7 +2955,7 @@ static bool osdDrawSingleElement(uint8_t item) STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); int digits = osdConfig()->plus_code_digits; int digitsRemoved = osdConfig()->plus_code_short * 2; - if (STATE(GPS_FIX)) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); } else { // +codes with > 8 digits have a + at the 9th digit diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 269ea0cd7bb..bc71851b549 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -784,7 +784,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff) timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 906db2881f0..5917ed16cb8 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2371,8 +2371,17 @@ bool validateRTHSanityChecker(void) return true; } + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; + //schedule check 5 seconds after apperange of real GPS fix, when position estimation coords stabilise after jump + posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; + return true; + } + // Check at 10Hz rate - if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) { + if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); posControl.rthSanityChecker.lastCheckTime = currentTimeMs; @@ -3889,7 +3898,7 @@ void updateWpMissionPlanner(void) { static timeMs_t resetTimerStart = 0; if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) { - const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX); + const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)); posControl.flags.wpMissionPlannerActive = true; if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d67d679992c..0b0a393a460 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -209,6 +209,8 @@ typedef struct positionEstimationConfig_s { float baro_epv; // Baro position error uint8_t use_gps_no_baro; + + uint8_t allow_gps_fix_estimation; } positionEstimationConfig_t; PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 511d92ef72d..6cfc48f5c59 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -697,7 +697,7 @@ bool isFixedWingLandingDetected(void) timeMs_t currentTimeMs = millis(); // Check horizontal and vertical volocities are low (cm/s) - bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 50.0f && posControl.actualState.velXY < 100.0f; + bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 50.0f && ((posControl.actualState.velXY < 100.0f) || STATE(GPS_ESTIMATED_FIX)); // Check angular rates are low (degs/s) bool gyroCondition = averageAbsGyroRates() < 2.0f; DEBUG_SET(DEBUG_LANDING, 2, velCondition); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index bd082da8085..0d6f7f28c15 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -91,7 +91,9 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .w_acc_bias = SETTING_INAV_W_ACC_BIAS_DEFAULT, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, - .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT + .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, + + .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT ); #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } @@ -150,6 +152,13 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs) bool isGlitching = false; + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + previousTime = 0; + return true; + } + if (previousTime == 0) { isGlitching = false; } @@ -202,7 +211,7 @@ void onNewGPSData(void) newLLH.alt = gpsSol.llh.alt; if (sensors(SENSOR_GPS)) { - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { isFirstGPSUpdate = true; return; } diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 0896f412de6..5c0a1a73143 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -76,7 +76,7 @@ uint16_t frskyGetGPSState(void) tmpi += (9 - constrain((gpsSol.hdop - 51) / 50, 0, 9)) * 100; // thousands column (GPS fix status) - if (STATE(GPS_FIX)) + if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) tmpi += 1000; if (STATE(GPS_FIX_HOME)) tmpi += 2000; diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index 0bc65d3457d..6d44ea7cf56 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -181,7 +181,7 @@ static void sendGpsAltitude(void) { uint16_t altitude = gpsSol.llh.alt / 100; // meters //Send real GPS altitude only if it's reliable (there's a GPS fix) - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { altitude = 0; } sendDataHead(ID_GPS_ALTIDUTE_BP); @@ -221,7 +221,7 @@ static void sendSatalliteSignalQualityAsTemperature2(void) static void sendSpeed(void) { - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { return; } //Speed should be sent in knots (GPS speed is in cm/s) @@ -234,7 +234,7 @@ static void sendSpeed(void) static void sendHomeDistance(void) { - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { return; } sendDataHead(ID_HOME_DIST); diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index 85f6f85b962..a9630efa317 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -147,7 +147,7 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) sbufWriteU16(dst, GPS_directionToHome); uint8_t gpsFlags = 0; - if (STATE(GPS_FIX)) { + if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { gpsFlags |= GPS_FLAGS_FIX; } if (STATE(GPS_FIX_HOME)) { diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 9e1f9f36226..92a84c15a06 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -234,7 +234,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120); hottGPSMessage->climbrate3s = climbrate3s & 0xFF; - if (!STATE(GPS_FIX)) { + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index 7fda28cd08b..a7c3776e4ee 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -348,7 +348,7 @@ static void sendSMS(void) memset(pluscode_url, 0, sizeof(pluscode_url)); - if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + if (sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { groundSpeed = gpsSol.groundSpeed / 100; char buf[20]; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 766dea19010..3b4e0277413 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -346,7 +346,7 @@ static bool smartPortShouldSendGPSData(void) // or the craft has never been armed yet. This way if GPS stops working // while in flight, the user will easily notice because the sensor will stop // updating. - return feature(FEATURE_GPS) && (STATE(GPS_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); + return feature(FEATURE_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); } void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index 8e01e47ef8e..d8a8c15c33a 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -303,7 +303,7 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs) uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint8_t hdopBcd, gpsFlags; - if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { + if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) || gpsSol.numSat < 6) { return false; } @@ -371,7 +371,7 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs) uint8_t numSatBcd, altitudeHighBcd; bool timeProvided = false; - if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) { + if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))|| gpsSol.numSat < 6) { return false; } From 00b50852fc3a52b02de01df5e93972571a4c54a2 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 21 Aug 2022 23:32:13 +0300 Subject: [PATCH 002/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 4cf9f010837..cfe943fda5f 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -6,7 +6,7 @@ Video demonstration There is possibility to allow plane to estimate it's position when GPS fix is lost. The main purpose is RTH without GPS. -It works on fixed wing only. +It works for fixed wing only. Plane should have the following sensors: - acceleromenter, gyroscope @@ -59,7 +59,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 m/s ```set fw_reference_airspeed=2777``` -*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Icrease cruise speed with throttle" - do not use it without GPS Fix.* +*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Increase cruise speed with throttle" - do not use it without GPS Fix.* *If pitot is available, pitot sensor data will be used instead of constant.* From ccfa58eec6d318cba2e3cafca97fd6af7ba64905 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 00:35:36 +0300 Subject: [PATCH 003/199] included fixed USER3 BOX ID collision --- src/main/fc/fc_msp_box.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 3d5459e9d9b..7fb1cd9be0f 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -87,14 +87,14 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXUSER1, .boxName = "USER1", .permanentId = BOX_PERMANENT_ID_USER1 }, { .boxId = BOXUSER2, .boxName = "USER2", .permanentId = BOX_PERMANENT_ID_USER2 }, { .boxId = BOXUSER3, .boxName = "USER3", .permanentId = BOX_PERMANENT_ID_USER3 }, - { .boxId = BOXLOITERDIRCHN, .boxName = "LOITER CHANGE", .permanentId = 57 }, - { .boxId = BOXMSPRCOVERRIDE, .boxName = "MSP RC OVERRIDE", .permanentId = 50 }, - { .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 51 }, - { .boxId = BOXTURTLE, .boxName = "TURTLE", .permanentId = 52 }, - { .boxId = BOXNAVCRUISE, .boxName = "NAV CRUISE", .permanentId = 53 }, - { .boxId = BOXAUTOLEVEL, .boxName = "AUTO LEVEL", .permanentId = 54 }, - { .boxId = BOXPLANWPMISSION, .boxName = "WP PLANNER", .permanentId = 55 }, - { .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 56 }, + { .boxId = BOXLOITERDIRCHN, .boxName = "LOITER CHANGE", .permanentId = 50 }, + { .boxId = BOXMSPRCOVERRIDE, .boxName = "MSP RC OVERRIDE", .permanentId = 51 }, + { .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 52 }, + { .boxId = BOXTURTLE, .boxName = "TURTLE", .permanentId = 53 }, + { .boxId = BOXNAVCRUISE, .boxName = "NAV CRUISE", .permanentId = 54 }, + { .boxId = BOXAUTOLEVEL, .boxName = "AUTO LEVEL", .permanentId = 55 }, + { .boxId = BOXPLANWPMISSION, .boxName = "WP PLANNER", .permanentId = 56 }, + { .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 57 }, { .boxId = BOXGPSOFF, .boxName = "GPS OFF", .permanentId = 58 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; From ffa28d89ffd3a213103f3c0811b5c4c119804501 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 16:01:45 +0300 Subject: [PATCH 004/199] updated documentation --- docs/Settings.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index ca0d7b90d8f..b28c5a1e25a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1842,6 +1842,16 @@ Defines if inav will dead-reckon over short GPS outages. May also be useful for --- +### inav_allow_gps_fix_estimation + +Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### inav_auto_mag_decl Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. From 7de16ffb3f27c59540b19dd0b8f6ebea9048a38a Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 16:10:20 +0300 Subject: [PATCH 005/199] retrigger checks From 2f6a1927d7135ee99db3b4dd9718ba791c9f38f8 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 20:10:08 +0300 Subject: [PATCH 006/199] retrigger checks From 9f0489e82da3c94a539787432b325065720b3153 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 21:19:15 +0300 Subject: [PATCH 007/199] retrigger checks From e4ff863bdc8ca7cbb85c2589f999c3e1ae9755cc Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 21:31:27 +0300 Subject: [PATCH 008/199] fixed rename getAirspeedEstimate() --- src/main/io/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 23fd9254a8e..abc44c601fa 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -215,7 +215,7 @@ void updateEstimatedGPSFix(void) { #ifdef USE_PITOT if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - speed = pitotCalculateAirSpeed(); + speed = getAirspeedEstimate(); } #endif From ccad410979abc887b3c0ecfe6ac78c53dcdfc439 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 21:41:16 +0300 Subject: [PATCH 009/199] retrigger checks From bd5a92cf005bb72b0ec83ca35623f9ebcaa61bcd Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 22 Aug 2022 21:41:31 +0300 Subject: [PATCH 010/199] retrigger checks From 2b83b16a89d04328e933ee8fb9c87c18f8b2d72d Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 26 Sep 2022 18:15:45 +0300 Subject: [PATCH 011/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index cfe943fda5f..60ea7818e82 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -50,12 +50,12 @@ Also you have to specify criuse airspeed of the plane. The get cruise airspeed, make test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. -Cruise airspeed is specified in m/s. +Cruise airspeed is specified in cm/s. To convert km/h to m/s, multiply by 27.77. -Example: 100 km/h = 100 * 27.77 = 2777 m/s +Example: 100 km/h = 100 * 27.77 = 2777 cm/s ```set fw_reference_airspeed=2777``` From a439e2925cb710e98fa30850ccc0c5dd082deeff Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 10 Dec 2022 19:52:18 +0200 Subject: [PATCH 012/199] adjusted docs --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index baa3338c368..d34e684d345 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -30,9 +30,9 @@ Without GPS fix, plane has nose heading from magnetometer only. To navigate without GPS fix, we make the following assumptions: - plane is flying in the direction where nose is pointing -- plane is flying with constant speed, specified in settings +- (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings -It is posible to roughtly estimate position using these assumptions. To increase heading accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase speed estimation accuracy, plane will use pitot tube (if available). +It is posible to roughtly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. From 3ade7e218dcc63a9930f6cc20a621dda7273fb46 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 28 Dec 2022 03:07:26 +0200 Subject: [PATCH 013/199] fixed typo in comment --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 6eaf8cec538..5fa68da903a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2385,7 +2385,7 @@ bool validateRTHSanityChecker(void) //disable sanity checks in GPS estimation mode //when estimated GPS fix is replaced with real fix, coordinates may jump posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; - //schedule check 5 seconds after apperange of real GPS fix, when position estimation coords stabilise after jump + //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; return true; } From 01ada270cd0c015fbdfe6af38a0bc351ae580455 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 28 Dec 2022 03:25:00 +0200 Subject: [PATCH 014/199] added failsafe_gps_fix_estimation_delay --- docs/Settings.md | 12 +++++++++++- src/main/fc/settings.yaml | 7 ++++++- src/main/flight/failsafe.c | 13 +++++++++++++ src/main/flight/failsafe.h | 2 ++ 4 files changed, 32 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index d1d339bfd6f..46f0f8ea43b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -852,6 +852,16 @@ Requested yaw rate to execute when `LAND` (or old `SET-THR`) failsafe is active --- +### failsafe_gps_fix_estimation_delay + +Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. + +| Default | Min | Max | +| --- | --- | --- | +| 7 | -1 | 600 | + +--- + ### failsafe_lights Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. @@ -1714,7 +1724,7 @@ Defines if INAV will dead-reckon over short GPS outages. May also be useful for ### inav_allow_gps_fix_estimation -Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS. +Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 15b05bf3c6d..92f0b109ebc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -840,6 +840,11 @@ groups: default_value: 0 min: -1 max: 600 + - name: failsafe_gps_fix_estimation_delay + description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation." + default_value: 7 + min: -1 + max: 600 - name: PG_LIGHTS_CONFIG type: lightsConfig_t @@ -2189,7 +2194,7 @@ groups: field: allow_dead_reckoning type: bool - name: inav_allow_gps_fix_estimation - description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS." + description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay." default_value: OFF field: allow_gps_fix_estimation type: bool diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 79ea091af3d..646cf92b3ec 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -82,6 +82,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s) + .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied ); typedef enum { @@ -403,6 +404,18 @@ void failsafeUpdateState(void) } reprocessState = true; } + else { + if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { + if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); + } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); + } + } else + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; + } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (!receivingRxDataAndNotFailsafeMode) { diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 7cd88acd591..0cbeffb8198 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -43,6 +43,7 @@ typedef struct failsafeConfig_s { uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) + int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); @@ -149,6 +150,7 @@ typedef struct failsafeState_s { timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time + timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation failsafeProcedure_e activeProcedure; failsafePhase_e phase; failsafeRxLinkState_e rxLinkState; From dfd9dff9e265dd33b45e61e6c83d443254ff2fa8 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 30 Dec 2022 01:36:46 +0200 Subject: [PATCH 015/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index d34e684d345..1972245e75b 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -48,7 +48,7 @@ GPS Fix estimation is enabled with CLI command: Also you have to specify criuse airspeed of the plane. -The get cruise airspeed, make test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. +To find out cruise airspeed, make a test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. Cruise airspeed is specified in cm/s. @@ -77,6 +77,12 @@ For testing purpoces, it is possible to disable GPS sensor fix from RC controlle *GPS can be disabled only after 1) initial GPS fix is acquired 2) in ARMED mode.* +# Allowing wp missions with GPS Fix estimation + +```failsafe_gps_fix_estimation_delay``` + +Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. + # Is it possible to implement this for multirotor ? There are some ideas, but there is no solution now. We can not make assumptions with multirotor which we can make with a fixed wing. From bf3a85700bc28fec2ade9b0a15405850f39cde17 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 15 Feb 2023 16:42:48 +0200 Subject: [PATCH 016/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 1972245e75b..f93b6fec3fc 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -63,7 +63,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 cm/s *If pitot is available, pitot sensor data will be used instead of constant.* -*Note related command: to continue mission without RC signal, see command ```set failsafe_mission=OFF```.* +*Note related command: to continue mission without RC signal, see command ```set failsafe_mission_delay=-1```.* **After entering CLI command, make sure that settings are saved:** From dc7119474354a8ddc103d63816d6d8f3ac738878 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 27 Feb 2023 13:39:26 +0100 Subject: [PATCH 017/199] show pitot failure status on osd --- src/main/io/osd.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1dd3ed4be82..c75c0c97051 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2678,12 +2678,20 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_AIR_SPEED: { #ifdef USE_PITOT - const float airspeed_estimate = getAirspeedEstimate(); buff[0] = SYM_AIR; - osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); - if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || - (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { + if (pitotIsHealthy()) + { + const float airspeed_estimate = getAirspeedEstimate(); + osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); + if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || + (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + else + { + strcpy(buff + 1, " X!"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } #else From 7591ed550a3bfc58a8c4bdf653527e26e9074031 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 27 Feb 2023 00:03:01 +0100 Subject: [PATCH 018/199] enable possibility to simulate GPS sensor timeout, mag failure and pitot failure with HITL --- src/main/fc/fc_msp.c | 6 +++++- src/main/fc/runtime_config.h | 5 ++++- src/main/io/gps.c | 19 +++++++++++++++---- src/main/sensors/diagnostics.c | 9 +++++++-- src/main/sensors/pitotmeter.c | 9 ++++++++- 5 files changed, 39 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ee5c8f03976..a5a634b4fa6 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3545,7 +3545,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (sensors(SENSOR_MAG)) { mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT - mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; + mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; } else { sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); @@ -3560,6 +3560,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { simulatorData.airSpeed = sbufReadU16(src); } + + if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { + simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; + } } else { DISABLE_STATE(GPS_FIX); } diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 779935b1277..522fcf73b6e 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -181,7 +181,10 @@ typedef enum { HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) HITL_HAS_NEW_GPS_DATA = (1 << 4), HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 - HITL_AIRSPEED = (1 << 6) + HITL_AIRSPEED = (1 << 6), + HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2 + HITL_GPS_TIMEOUT = (1 << 8), + HITL_PITOT_FAILURE = (1 << 9) } simulatorFlags_t; typedef struct { diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 4976b797bea..0696ae0b62d 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -474,10 +474,21 @@ bool gpsUpdate(void) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE)) { - gpsUpdateTime(); - gpsSetState(GPS_RUNNING); - sensorsSet(SENSOR_GPS); - return gpsSol.flags.hasNewData; + if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) + { + gpsSetState(GPS_LOST_COMMUNICATION); + sensorsClear(SENSOR_GPS); + gpsStats.timeouts = 5; + return false; + } + else + { + gpsSetState(GPS_RUNNING); + sensorsSet(SENSOR_GPS); + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; + } } #endif #ifdef USE_FAKE_GPS diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 0702f78991e..d0ae8324fb2 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -62,12 +62,17 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void) hardwareSensorStatus_e getHwCompassStatus(void) { +#if defined(USE_MAG) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE) && sensors(SENSOR_MAG)) { - return HW_SENSOR_OK; + if (compassIsHealthy()) { + return HW_SENSOR_OK; + } + else { + return HW_SENSOR_UNHEALTHY; + } } #endif -#if defined(USE_MAG) if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { return HW_SENSOR_OK; diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index eb2cad13f03..7c8fb7ad29b 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -193,6 +193,13 @@ STATIC_PROTOTHREAD(pitotThread) pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f); while(1) { +#ifdef USE_SIMULATOR + while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE)) + { + ptDelayUs(10000); + } +#endif + // Start measurement if (pitot.dev.start(&pitot.dev)) { pitot.lastSeenHealthyMs = millis(); @@ -236,7 +243,7 @@ STATIC_PROTOTHREAD(pitotThread) } #ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitot.airSpeed = simulatorData.airSpeed; + pitot.airSpeed = simulatorData.airSpeed; } #endif } From e6027076f994e32d5feaed449fcb8c1a53685831 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 5 Mar 2023 23:21:13 +0100 Subject: [PATCH 019/199] fix GPS timeout display on OSD --- src/main/io/osd.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c75c0c97051..850701573f0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1707,9 +1707,11 @@ static bool osdDrawSingleElement(uint8_t item) if (STATE(GPS_ESTIMATED_FIX)) { if (!STATE(GPS_FIX)) { strcpy(buff + 2, "ES"); - } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { - strcpy(buff + 2, "X!"); - } + } + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + strcpy(buff + 2, "X!"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; From ce9cbd1f5d4ac94253719a9e8dcdfce41e9db5a0 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 5 Mar 2023 23:21:51 +0100 Subject: [PATCH 020/199] estimate GPS Fix only if compass and barometer are healthy --- src/main/io/gps.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 0696ae0b62d..d26bb691a8b 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -152,7 +152,11 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) bool canEstimateGPSFix(void) { - return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_GPS) && sensors(SENSOR_BARO) && sensors(SENSOR_MAG) && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); + return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && + sensors(SENSOR_GPS) && + sensors(SENSOR_BARO) && baroIsHealthy() && + sensors(SENSOR_MAG) && compassIsHealthy() && + ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); } void updateEstimatedGPSFix(void) { From 12cb0c57571ea48a794a2aab89fb6b5a3e507267 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 17:56:36 +0100 Subject: [PATCH 021/199] support GPS Fix estimation on sensor timeout --- src/main/flight/failsafe.c | 2 +- src/main/flight/imu.c | 1 + src/main/io/gps.c | 67 +++++++++++++++---- .../navigation/navigation_pos_estimator.c | 6 +- src/main/telemetry/frsky_d.c | 2 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/ibus_shared.c | 32 ++++----- src/main/telemetry/jetiexbus.c | 2 +- src/main/telemetry/ltm.c | 2 +- src/main/telemetry/mavlink.c | 4 +- src/main/telemetry/sim.c | 2 +- 11 files changed, 81 insertions(+), 41 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index d96730f78ab..35c10dc3153 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -355,7 +355,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set if (failsafeConfig()->failsafe_min_distance > 0 && - sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { + ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { // get the distance to the original arming point uint32_t distance = calculateDistanceToDestination(&original_rth_home); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 643773cebcc..5dbe717c18d 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -321,6 +321,7 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c #endif } +//NOTE: checks if real GPS data is present, ignores any available GPS Fix estimation bool isGPSTrustworthy(void) { return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index d26bb691a8b..d63313099c6 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -152,23 +152,16 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) bool canEstimateGPSFix(void) { + //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: + //1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once + //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && - sensors(SENSOR_GPS) && sensors(SENSOR_BARO) && baroIsHealthy() && sensors(SENSOR_MAG) && compassIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); } -void updateEstimatedGPSFix(void) { - - static uint32_t lastUpdateMs = 0; - static int32_t estimated_lat = 0; - static int32_t estimated_lon = 0; - static int32_t estimated_alt = 0; - - uint32_t t = millis(); - int32_t dt = t - lastUpdateMs; - lastUpdateMs = t; +void processDisableGPSFix(void) { static int32_t last_lat = 0; static int32_t last_lon = 0; @@ -180,6 +173,11 @@ void updateEstimatedGPSFix(void) { gpsSol.hdop = 9999; gpsSol.numSat = 0; + gpsSol.flags.validVelNE = false; + gpsSol.flags.validVelD = false; + gpsSol.flags.validEPE = false; + gpsSol.flags.validTime = false; + //freeze coordinates gpsSol.llh.lat = last_lat; gpsSol.llh.lon = last_lon; @@ -193,6 +191,18 @@ void updateEstimatedGPSFix(void) { last_lon = gpsSol.llh.lon; last_alt = gpsSol.llh.alt; } +} + +void updateEstimatedGPSFix(void) { + + static uint32_t lastUpdateMs = 0; + static int32_t estimated_lat = 0; + static int32_t estimated_lon = 0; + static int32_t estimated_alt = 0; + + uint32_t t = millis(); + int32_t dt = t - lastUpdateMs; + lastUpdateMs = t; if (STATE(GPS_FIX) || !canEstimateGPSFix()) { DISABLE_STATE(GPS_ESTIMATED_FIX); @@ -212,9 +222,10 @@ void updateEstimatedGPSFix(void) { gpsSol.eph = 100; gpsSol.epv = 100; - gpsSol.flags.validVelNE = 1; - gpsSol.flags.validVelD = 0; //do not provide velocity.z - gpsSol.flags.validEPE = 1; + gpsSol.flags.validVelNE = true; + gpsSol.flags.validVelD = false; //do not provide velocity.z + gpsSol.flags.validEPE = true; + gpsSol.flags.validTime = false; float speed = pidProfile()->fixedWingReferenceAirspeed; @@ -281,6 +292,8 @@ void gpsProcessNewSolutionData(void) // Set sensor as ready and available sensorsSet(SENSOR_GPS); + processDisableGPSFix(); + updateEstimatedGPSFix(); // Pass on GPS update to NAV and IMU @@ -298,6 +311,8 @@ void gpsProcessNewSolutionData(void) // Toggle heartbeat gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; + + debug[0]+=1; } static void gpsResetSolution(void) @@ -316,6 +331,23 @@ static void gpsResetSolution(void) gpsSol.flags.validTime = false; } +void gpsTryEstimateOnTimeout(void) +{ + gpsResetSolution(); + DISABLE_STATE(GPS_FIX); + + processDisableGPSFix(); + + updateEstimatedGPSFix(); + + if (STATE(GPS_ESTIMATED_FIX)) + { + onNewGPSData(); + gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; + } + debug[1]+=1; +} + void gpsPreInit(void) { // Make sure gpsProvider is known when gpsMagDetect is called @@ -483,6 +515,7 @@ bool gpsUpdate(void) gpsSetState(GPS_LOST_COMMUNICATION); sensorsClear(SENSOR_GPS); gpsStats.timeouts = 5; + gpsTryEstimateOnTimeout(); return false; } else @@ -542,6 +575,11 @@ bool gpsUpdate(void) break; } + if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) + { + gpsTryEstimateOnTimeout(); + } + return gpsSol.flags.hasNewData; #endif } @@ -588,6 +626,7 @@ bool isGPSHealthy(void) return true; } +//NOTE: checks if real GPS data present, ignoring any available GPS Fix estimation bool isGPSHeadingValid(void) { return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 6363fc2a0f8..91cd96a85b2 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -210,7 +210,7 @@ void onNewGPSData(void) newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { isFirstGPSUpdate = true; return; @@ -486,7 +486,7 @@ static bool navIsAccelerationUsable(void) static bool navIsHeadingUsable(void) { - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { // If we have GPS - we need true IMU north (valid heading) return isImuHeadingValid(); } @@ -501,7 +501,7 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) /* Figure out if we have valid position data from our data sources */ uint32_t newFlags = 0; - if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid && + if ((sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index e55aa0631a0..73e8e9801e8 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -540,7 +540,7 @@ void handleFrSkyTelemetry(void) } #ifdef USE_GPS - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { sendSpeed(); sendHomeDistance(); sendGpsAltitude(); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 92a84c15a06..a9810eb8f6d 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -476,7 +476,7 @@ static bool processBinaryModeRequest(uint8_t address) switch (address) { #ifdef USE_GPS case 0x8A: - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { hottPrepareGPSResponse(&hottGPSMessage); hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage)); return true; diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 1e598dd2a76..a8d0cd82d0f 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -135,7 +135,7 @@ static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8 static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { #if defined(USE_GPS) uint8_t fix = 0; - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (gpsSol.fixType == GPS_NO_FIX) fix = 1; else if (gpsSol.fixType == GPS_FIX_2D) fix = 2; else if (gpsSol.fixType == GPS_FIX_3D) fix = 3; @@ -195,7 +195,7 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0 uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()]; #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { status += gpsSol.numSat * 1000; status += fix * 100; if (STATE(GPS_FIX_HOME)) status += 500; @@ -205,72 +205,72 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { return sendIbusMeasurement2(address, status); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north) #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m #if defined(USE_GPS) - if (sensors(SENSOR_GPS)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t #endif return sendIbusMeasurement2(address, 0); } diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index c9b68fa27f7..53de96cc5d4 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -575,7 +575,7 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); if (!allSensorsActive) { - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { enableGpsTelemetry(true); allSensorsActive = true; } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 282c9be4fd2..220623fc2c4 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -117,7 +117,7 @@ void ltm_gframe(sbuf_t *dst) uint8_t gps_fix_type = 0; int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0; - if (sensors(SENSOR_GPS)) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (gpsSol.fixType == GPS_NO_FIX) gps_fix_type = 1; else if (gpsSol.fixType == GPS_FIX_2D) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 58a96a92d45..8be4aa72b21 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -523,7 +523,7 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) { uint8_t gpsFixType = 0; - if (!sensors(SENSOR_GPS)) + if (!(sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) return; if (gpsSol.fixType == GPS_NO_FIX) @@ -638,7 +638,7 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_GPS) // use ground speed if source available - if (sensors(SENSOR_GPS)) { + if (!( sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) { mavGroundSpeed = gpsSol.groundSpeed / 100.0f; } #endif diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index 85f1e740d6b..9be24ed7678 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -348,7 +348,7 @@ static void sendSMS(void) ZERO_FARRAY(pluscode_url); - if (sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { + if ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) { groundSpeed = gpsSol.groundSpeed / 100; char buf[20]; From a62bc745ff6c28a5584eaedc2e5b95852a0be6f8 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 19:03:41 +0200 Subject: [PATCH 022/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index f93b6fec3fc..4f43eed353d 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -21,6 +21,8 @@ GPS fix estimation allows to recover plane using magnetometer and baromener only Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. +GPS Fix is also estimated on GPS Sensor timeout (harware failure). + # How it works ? In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints. @@ -40,6 +42,7 @@ It is assumed, that plane will fly in roughtly estimated direction to home posit *Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. + # Settings GPS Fix estimation is enabled with CLI command: @@ -75,7 +78,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 cm/s For testing purpoces, it is possible to disable GPS sensor fix from RC controller in programming tab: -*GPS can be disabled only after 1) initial GPS fix is acquired 2) in ARMED mode.* +*GPS can be disabled only after: 1) initial GPS fix is acquired 2) in ARMED mode.* # Allowing wp missions with GPS Fix estimation From 6e7dafb96d12889af47226c4f578585b88a517cc Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 18:37:40 +0100 Subject: [PATCH 023/199] fixed compilation error --- src/main/io/gps.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index d63313099c6..71b9e964187 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -152,6 +152,8 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) bool canEstimateGPSFix(void) { +#if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) + //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: //1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix @@ -159,6 +161,11 @@ bool canEstimateGPSFix(void) sensors(SENSOR_BARO) && baroIsHealthy() && sensors(SENSOR_MAG) && compassIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); + +#else + return false; +#endif + } void processDisableGPSFix(void) { From 7afaa0db34dd12132cf57f3e8ccf511b41ac44db Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 20:18:11 +0100 Subject: [PATCH 024/199] fixed indentation --- src/main/flight/failsafe.h | 2 +- src/main/flight/wind_estimator.c | 5 +- src/main/io/gps.c | 203 ++++++++++----------- src/main/io/osd.c | 6 +- src/main/navigation/navigation.c | 16 +- src/main/navigation/navigation_fixedwing.c | 6 +- 6 files changed, 115 insertions(+), 123 deletions(-) diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 0cbeffb8198..d80ab650c78 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -43,7 +43,7 @@ typedef struct failsafeConfig_s { uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) - int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) + int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index e4af1828190..30f15fac480 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -83,8 +83,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) static float lastValidEstimateAltitude = 0.0f; float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m - if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) - { + if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) { hasValidWindEstimate = false; } @@ -92,7 +91,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) !isGPSHeadingValid() || !gpsSol.flags.validVelNE || !gpsSol.flags.validVelD || - STATE(GPS_ESTIMATED_FIX)){ + STATE(GPS_ESTIMATED_FIX)) { return; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 2f954e4ba93..28ef5496517 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -163,121 +163,116 @@ bool canEstimateGPSFix(void) ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); #else - return false; + return false; #endif } -void processDisableGPSFix(void) { - - static int32_t last_lat = 0; - static int32_t last_lon = 0; - static int32_t last_alt = 0; +void processDisableGPSFix(void) +{ + static int32_t last_lat = 0; + static int32_t last_lon = 0; + static int32_t last_alt = 0; - if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX)) - { - gpsSol.fixType = GPS_NO_FIX; - gpsSol.hdop = 9999; - gpsSol.numSat = 0; + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX)) { + gpsSol.fixType = GPS_NO_FIX; + gpsSol.hdop = 9999; + gpsSol.numSat = 0; - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - gpsSol.flags.validEPE = false; + gpsSol.flags.validVelNE = false; + gpsSol.flags.validVelD = false; + gpsSol.flags.validEPE = false; gpsSol.flags.validTime = false; - //freeze coordinates - gpsSol.llh.lat = last_lat; - gpsSol.llh.lon = last_lon; - gpsSol.llh.alt = last_alt; - - DISABLE_STATE(GPS_FIX); - } - else - { - last_lat = gpsSol.llh.lat; - last_lon = gpsSol.llh.lon; - last_alt = gpsSol.llh.alt; - } + //freeze coordinates + gpsSol.llh.lat = last_lat; + gpsSol.llh.lon = last_lon; + gpsSol.llh.alt = last_alt; + + DISABLE_STATE(GPS_FIX); + } else { + last_lat = gpsSol.llh.lat; + last_lon = gpsSol.llh.lon; + last_alt = gpsSol.llh.alt; + } } -void updateEstimatedGPSFix(void) { - - static uint32_t lastUpdateMs = 0; - static int32_t estimated_lat = 0; - static int32_t estimated_lon = 0; - static int32_t estimated_alt = 0; - - uint32_t t = millis(); - int32_t dt = t - lastUpdateMs; - lastUpdateMs = t; - - if (STATE(GPS_FIX) || !canEstimateGPSFix()) { - DISABLE_STATE(GPS_ESTIMATED_FIX); - estimated_lat = gpsSol.llh.lat; - estimated_lon = gpsSol.llh.lon; - estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; - return; - } - - ENABLE_STATE(GPS_ESTIMATED_FIX); - - gpsSol.fixType = GPS_FIX_3D; - gpsSol.hdop = 99; - gpsSol.flags.hasNewData = true; - gpsSol.numSat = 99; - - gpsSol.eph = 100; - gpsSol.epv = 100; - - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = false; //do not provide velocity.z - gpsSol.flags.validEPE = true; +void updateEstimatedGPSFix(void) +{ + static uint32_t lastUpdateMs = 0; + static int32_t estimated_lat = 0; + static int32_t estimated_lon = 0; + static int32_t estimated_alt = 0; + + uint32_t t = millis(); + int32_t dt = t - lastUpdateMs; + lastUpdateMs = t; + + if (STATE(GPS_FIX) || !canEstimateGPSFix()) { + DISABLE_STATE(GPS_ESTIMATED_FIX); + estimated_lat = gpsSol.llh.lat; + estimated_lon = gpsSol.llh.lon; + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + return; + } + + ENABLE_STATE(GPS_ESTIMATED_FIX); + + gpsSol.fixType = GPS_FIX_3D; + gpsSol.hdop = 99; + gpsSol.flags.hasNewData = true; + gpsSol.numSat = 99; + + gpsSol.eph = 100; + gpsSol.epv = 100; + + gpsSol.flags.validVelNE = true; + gpsSol.flags.validVelD = false; //do not provide velocity.z + gpsSol.flags.validEPE = true; gpsSol.flags.validTime = false; - float speed = pidProfile()->fixedWingReferenceAirspeed; + float speed = pidProfile()->fixedWingReferenceAirspeed; #ifdef USE_PITOT - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) - { - speed = getAirspeedEstimate(); - } + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + speed = getAirspeedEstimate(); + } #endif - float velX = rMat[0][0] * speed; - float velY = -rMat[1][0] * speed; - // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame - - if (isEstimatedWindSpeedValid()) { - velX += getEstimatedWindSpeed(X); - velY += getEstimatedWindSpeed(Y); - } - // here (velX, velY) is estimated horizontal speed with wind influence = ground speed - - if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent() == 0))) - { - velX = 0; - velY = 0; - } - - estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) ); - estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) ); - estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; - - gpsSol.llh.lat = estimated_lat; - gpsSol.llh.lon = estimated_lon; - gpsSol.llh.alt = estimated_alt; - - gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY); - - float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction - if (groundCourse < 0) { - groundCourse += 2 * M_PIf; - } - gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse); - - gpsSol.velNED[X] = (int16_t)(velX); - gpsSol.velNED[Y] = (int16_t)(velY); - gpsSol.velNED[Z] = 0; + float velX = rMat[0][0] * speed; + float velY = -rMat[1][0] * speed; + // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame + + if (isEstimatedWindSpeedValid()) { + velX += getEstimatedWindSpeed(X); + velY += getEstimatedWindSpeed(Y); + } + // here (velX, velY) is estimated horizontal speed with wind influence = ground speed + + if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent() == 0))) { + velX = 0; + velY = 0; + } + + estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) ); + estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) ); + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + + gpsSol.llh.lat = estimated_lat; + gpsSol.llh.lon = estimated_lon; + gpsSol.llh.alt = estimated_alt; + + gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY); + + float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction + if (groundCourse < 0) { + groundCourse += 2 * M_PIf; + } + gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse); + + gpsSol.velNED[X] = (int16_t)(velX); + gpsSol.velNED[Y] = (int16_t)(velY); + gpsSol.velNED[Z] = 0; } @@ -301,7 +296,7 @@ void gpsProcessNewSolutionData(void) processDisableGPSFix(); - updateEstimatedGPSFix(); + updateEstimatedGPSFix(); // Pass on GPS update to NAV and IMU onNewGPSData(); @@ -345,10 +340,9 @@ void gpsTryEstimateOnTimeout(void) processDisableGPSFix(); - updateEstimatedGPSFix(); + updateEstimatedGPSFix(); - if (STATE(GPS_ESTIMATED_FIX)) - { + if (STATE(GPS_ESTIMATED_FIX)) { onNewGPSData(); gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; } @@ -582,8 +576,7 @@ bool gpsUpdate(void) break; } - if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) - { + if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) { gpsTryEstimateOnTimeout(); } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 850701573f0..b1081a92a2b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1705,9 +1705,9 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); if (STATE(GPS_ESTIMATED_FIX)) { - if (!STATE(GPS_FIX)) { - strcpy(buff + 2, "ES"); - } + if (!STATE(GPS_FIX)) { + strcpy(buff + 2, "ES"); + } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 58dda7f8086..40cc7989c71 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2376,14 +2376,14 @@ bool validateRTHSanityChecker(void) return true; } - if (STATE(GPS_ESTIMATED_FIX)) { - //disable sanity checks in GPS estimation mode - //when estimated GPS fix is replaced with real fix, coordinates may jump - posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; - //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump - posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; - return true; - } + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; + //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump + posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; + return true; + } // Check at 10Hz rate if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 34ca67d737c..09681047a0e 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -319,10 +319,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // turn direction to next waypoint loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right - needToCalculateCircularLoiter = true; - } + needToCalculateCircularLoiter = true; + } posControl.flags.wpTurnSmoothingActive = true; - } + } } // We are closing in on a waypoint, calculate circular loiter if required From 69a122099226ac1287474b8784bd2c5ecdd3d34d Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 20:30:28 +0100 Subject: [PATCH 025/199] fixed indentation --- src/main/fc/fc_msp.c | 56 +++++++++++----------- src/main/navigation/navigation_fixedwing.c | 2 +- src/main/sensors/diagnostics.c | 14 ++---- 3 files changed, 34 insertions(+), 38 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b2ebac8343f..04e55b4f503 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3222,8 +3222,8 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src) #ifdef USE_SIMULATOR bool isOSDTypeSupportedBySimulator(void) { - displayPort_t *osdDisplayPort = osdGetDisplayPort(); - return (osdDisplayPort && osdDisplayPort->cols == 30 && (osdDisplayPort->rows == 13 || osdDisplayPort->rows == 16)); + displayPort_t *osdDisplayPort = osdGetDisplayPort(); + return (osdDisplayPort && osdDisplayPort->cols == 30 && (osdDisplayPort->rows == 13 || osdDisplayPort->rows == 16)); } void mspWriteSimulatorOSD(sbuf_t *dst) @@ -3434,51 +3434,51 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #ifdef USE_SIMULATOR case MSP_SIMULATOR: - tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version + tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version // Check the MSP version of simulator - if (tmp_u8 != SIMULATOR_MSP_VERSION) { + if (tmp_u8 != SIMULATOR_MSP_VERSION) { break; } - simulatorData.flags = sbufReadU8(src); + simulatorData.flags = sbufReadU8(src); if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) { - if (ARMING_FLAG(SIMULATOR_MODE)) { // Just once - DISABLE_ARMING_FLAG(SIMULATOR_MODE); + if (ARMING_FLAG(SIMULATOR_MODE)) { // Just once + DISABLE_ARMING_FLAG(SIMULATOR_MODE); #ifdef USE_BARO - baroStartCalibration(); + baroStartCalibration(); #endif #ifdef USE_MAG - DISABLE_STATE(COMPASS_CALIBRATED); - compassInit(); + DISABLE_STATE(COMPASS_CALIBRATED); + compassInit(); #endif - simulatorData.flags = HITL_RESET_FLAGS; + simulatorData.flags = HITL_RESET_FLAGS; // Review: Many states were affected. Reboot? - disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! - } - } else if (!areSensorsCalibrating()) { - if (!ARMING_FLAG(SIMULATOR_MODE)) { // Just once + disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! + } + } else if (!areSensorsCalibrating()) { + if (!ARMING_FLAG(SIMULATOR_MODE)) { // Just once #ifdef USE_BARO - baroStartCalibration(); -#endif + baroStartCalibration(); +#endif #ifdef USE_MAG - if (compassConfig()->mag_hardware != MAG_NONE) { - sensorsSet(SENSOR_MAG); - ENABLE_STATE(COMPASS_CALIBRATED); - DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); - mag.magADC[X] = 800; - mag.magADC[Y] = 0; - mag.magADC[Z] = 0; - } + if (compassConfig()->mag_hardware != MAG_NONE) { + sensorsSet(SENSOR_MAG); + ENABLE_STATE(COMPASS_CALIBRATED); + DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); + mag.magADC[X] = 800; + mag.magADC[Y] = 0; + mag.magADC[Z] = 0; + } #endif - ENABLE_ARMING_FLAG(SIMULATOR_MODE); - LOG_DEBUG(SYSTEM, "Simulator enabled"); - } + ENABLE_ARMING_FLAG(SIMULATOR_MODE); + LOG_DEBUG(SYSTEM, "Simulator enabled"); + } if (dataSize >= 14) { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 09681047a0e..ca6e56bae25 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -406,7 +406,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - // courseVirtualCorrection initially used to determine current position relative to course line for later use + // courseVirtualCorrection initially used to determine current position relative to course line for later use int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 351d5c6b5f8..583f852224b 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -64,11 +64,10 @@ hardwareSensorStatus_e getHwCompassStatus(void) { #if defined(USE_MAG) #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE) && sensors(SENSOR_MAG)) { + if (ARMING_FLAG(SIMULATOR_MODE) && sensors(SENSOR_MAG)) { if (compassIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -76,17 +75,14 @@ hardwareSensorStatus_e getHwCompassStatus(void) if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } - } - else { + } else { if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } From f9e078f9c145ddd12ee36e1d8c40d9ef10379fff Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 21:44:38 +0100 Subject: [PATCH 026/199] fixed indentation --- src/main/drivers/display.c | 16 ++++++++-------- src/main/drivers/sound_beeper.c | 10 +++++----- src/main/navigation/navigation_pos_estimator.c | 12 ++++++------ 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 28bbd881338..f3802638adf 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -179,14 +179,14 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c) } #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE)) { - //some FCs do not power max7456 from USB power - //driver can not read font metadata - //chip assumed to not support second bank of font - //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar()) - //return dummy metadata to let all OSD elements to work in simulator mode - instance->maxChar = 512; - } + if (ARMING_FLAG(SIMULATOR_MODE)) { + //some FCs do not power max7456 from USB power + //driver can not read font metadata + //chip assumed to not support second bank of font + //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar()) + //return dummy metadata to let all OSD elements to work in simulator mode + instance->maxChar = 512; + } #endif if (c > instance->maxChar) { diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index bb089b4cb9e..4d7981b867f 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -47,11 +47,11 @@ void systemBeep(bool onoff) #else #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE)) { - if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) { - onoff = false; - } - } + if (ARMING_FLAG(SIMULATOR_MODE)) { + if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) { + onoff = false; + } + } #endif if (beeperConfig()->pwmMode) { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 91cd96a85b2..4d5139479de 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -152,12 +152,12 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs) bool isGlitching = false; - if (STATE(GPS_ESTIMATED_FIX)) { - //disable sanity checks in GPS estimation mode - //when estimated GPS fix is replaced with real fix, coordinates may jump - previousTime = 0; - return true; - } + if (STATE(GPS_ESTIMATED_FIX)) { + //disable sanity checks in GPS estimation mode + //when estimated GPS fix is replaced with real fix, coordinates may jump + previousTime = 0; + return true; + } if (previousTime == 0) { isGlitching = false; From 4c39d77c45eb0df07a3c75ccd1e100c171652963 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 21:44:48 +0100 Subject: [PATCH 027/199] removed debug code --- src/main/io/gps.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 28ef5496517..34a5f915a77 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -157,7 +157,7 @@ bool canEstimateGPSFix(void) //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: //1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix - return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && + return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_BARO) && baroIsHealthy() && sensors(SENSOR_MAG) && compassIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); @@ -313,8 +313,6 @@ void gpsProcessNewSolutionData(void) // Toggle heartbeat gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; - - debug[0]+=1; } static void gpsResetSolution(void) @@ -346,7 +344,6 @@ void gpsTryEstimateOnTimeout(void) onNewGPSData(); gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; } - debug[1]+=1; } void gpsPreInit(void) From 2ce2904d5f798dc0e385f6f11245420dc7c62b13 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 23:55:14 +0100 Subject: [PATCH 028/199] attempt to fix failsafe_gps_fix_estimation_delay bug --- src/main/flight/failsafe.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 35c10dc3153..8bf654a2f52 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -408,18 +408,19 @@ void failsafeUpdateState(void) failsafeState.wpModeDelayedFailsafeStart = 0; } reprocessState = true; - } - else { + } else { if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { - failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); - failsafeActivate(FAILSAFE_RETURN_TO_HOME); - activateForcedRTH(); + if ( !FLIGHT_MODE(FAILSAFE_MODE) ) { + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); + } } - } else - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; + } else + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) From a10b7ceaad3ab246a1114cfd1d704b5bfe813d96 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 8 Mar 2023 08:29:54 +0100 Subject: [PATCH 029/199] enable ground course estimation --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 4d5139479de..0f9e0582df3 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -698,7 +698,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) { - if (STATE(GPS_FIX) && navIsHeadingUsable()) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && navIsHeadingUsable()) { static timeUs_t lastUpdateTimeUs = 0; if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate From 20cdfc8935173c3ea74ee79fb666ca5664701252 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 8 Mar 2023 16:04:36 +0100 Subject: [PATCH 030/199] fixed landing detection with GPS fix estimation --- src/main/navigation/navigation_fixedwing.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ca6e56bae25..691467e3210 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -715,7 +715,7 @@ bool isFixedWingLandingDetected(void) // Check horizontal and vertical velocities are low (cm/s) bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) && - ( posControl.actualState.velXY < (100.0f * sensitivity) || STATE(GPS_ESTIMATED_FIX)); + ( posControl.actualState.velXY < (100.0f * sensitivity)); // Check angular rates are low (degs/s) bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity); DEBUG_SET(DEBUG_LANDING, 2, velCondition); From ec5e0c8a73a96f4ba523568fbdb992d815f43442 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 8 Mar 2023 23:53:12 +0100 Subject: [PATCH 031/199] fixed: forced RTH is not activated on GPS Fix estimation event during mission with RX loss --- src/main/flight/failsafe.c | 46 +++++++++++++++++++++++++------------- 1 file changed, 31 insertions(+), 15 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 8bf654a2f52..e2cd71cc9d7 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -368,6 +368,27 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) return failsafeConfig()->failsafe_procedure; } + +bool checkGPSFixFailsafe(void) +{ + if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { + if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); + } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { + if ( !posControl.flags.forcedRTHActivated ) { + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); + return true; + } + } + } else { + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; + } + return false; +} + + void failsafeUpdateState(void) { if (!failsafeIsMonitoring() || failsafeIsSuspended()) { @@ -396,7 +417,10 @@ void failsafeUpdateState(void) if (!throttleStickIsLow()) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } - if (!receivingRxDataAndNotFailsafeMode) { + + if ( checkGPSFixFailsafe() ) { + reprocessState = true; + } else if (!receivingRxDataAndNotFailsafeMode) { if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero @@ -408,20 +432,7 @@ void failsafeUpdateState(void) failsafeState.wpModeDelayedFailsafeStart = 0; } reprocessState = true; - } else { - if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { - if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); - } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { - if ( !FLIGHT_MODE(FAILSAFE_MODE) ) { - failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); - failsafeActivate(FAILSAFE_RETURN_TO_HOME); - activateForcedRTH(); - } - } - } else - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; - } + } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (!receivingRxDataAndNotFailsafeMode) { @@ -477,7 +488,12 @@ void failsafeUpdateState(void) } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; reprocessState = true; + } else { + if ( checkGPSFixFailsafe() ) { + reprocessState = true; + } } + break; case FAILSAFE_RETURN_TO_HOME: From 44fea0ece8a3ca1054e17c9d505f381f4d91b8f7 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 5 Apr 2023 13:02:06 +0200 Subject: [PATCH 032/199] fixed merge conflict --- src/main/io/osd.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e83f88a8291..bbf02b359d1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2916,7 +2916,6 @@ static bool osdDrawSingleElement(uint8_t item) bool moreThanAh = false; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { uint8_t digits = 3U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values if (isBfCompatibleVideoSystem(osdConfig())) { @@ -2924,8 +2923,8 @@ static bool osdDrawSingleElement(uint8_t item) digits = 4U; } #endif - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); From cc3991298471103e380ab8cabd3f5152e25baaa6 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 25 Apr 2023 11:56:15 +0200 Subject: [PATCH 033/199] fixed: groundspeed is not sent in mavlink telemetry --- src/main/telemetry/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 892c1d3b71c..37a342b94f6 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -638,7 +638,7 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_GPS) // use ground speed if source available - if (!( sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) { + if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { mavGroundSpeed = gpsSol.groundSpeed / 100.0f; } #endif From 9796bbbde55ee5705516c7d0857b7f870e042607 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 27 Apr 2023 14:46:59 +0200 Subject: [PATCH 034/199] use any wind estimator value with gps fix estimation --- src/main/flight/wind_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 30f15fac480..b6174611add 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -52,7 +52,7 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT]; bool isEstimatedWindSpeedValid(void) { - return hasValidWindEstimate; + return hasValidWindEstimate || STATE(GPS_ESTIMATED_FIX); //use any wind estimate with GPS fix estimation. } float getEstimatedWindSpeed(int axis) From 55fdeae002c3f02bc87f5433ae030e5df4f84fa1 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 27 Apr 2023 15:56:11 +0200 Subject: [PATCH 035/199] improved course hold with GPS Fix estimation --- src/main/io/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 283c0941bc8..e75193617ab 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -559,7 +559,7 @@ bool isGPSHealthy(void) //NOTE: checks if real GPS data present, ignoring any available GPS Fix estimation bool isGPSHeadingValid(void) { - return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300; + return sensors(SENSOR_GPS) && ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; } #endif From 4d888e438b542eac3b0ac1206cd87e7894ca8618 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 18:15:09 +0200 Subject: [PATCH 036/199] fixed gpsSol access race conditions --- src/main/fc/fc_msp.c | 47 ++++++++-------- src/main/io/gps.c | 90 ++++++++++++++++++------------- src/main/io/gps.h | 2 - src/main/io/gps_fake.c | 52 +++++++++--------- src/main/io/gps_msp.c | 55 +++++++++---------- src/main/io/gps_nmea.c | 51 +++++++++--------- src/main/io/gps_private.h | 2 + src/main/io/gps_ublox.c | 109 +++++++++++++++++++------------------- 8 files changed, 216 insertions(+), 192 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e4b01fab966..432ad6e6a7a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3513,35 +3513,36 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (dataSize >= 14) { if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { - gpsSol.fixType = sbufReadU8(src); - gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSol.flags.hasNewData = true; - gpsSol.numSat = sbufReadU8(src); - - if (gpsSol.fixType != GPS_NO_FIX) { - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - gpsSol.flags.validTime = false; - - gpsSol.llh.lat = sbufReadU32(src); - gpsSol.llh.lon = sbufReadU32(src); - gpsSol.llh.alt = sbufReadU32(src); - gpsSol.groundSpeed = (int16_t)sbufReadU16(src); - gpsSol.groundCourse = (int16_t)sbufReadU16(src); - - gpsSol.velNED[X] = (int16_t)sbufReadU16(src); - gpsSol.velNED[Y] = (int16_t)sbufReadU16(src); - gpsSol.velNED[Z] = (int16_t)sbufReadU16(src); - - gpsSol.eph = 100; - gpsSol.epv = 100; + gpsSolDRV.fixType = sbufReadU8(src); + gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.flags.hasNewData = true; + gpsSolDRV.numSat = sbufReadU8(src); + + if (gpsSolDRV.fixType != GPS_NO_FIX) { + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.validTime = false; + + gpsSolDRV.llh.lat = sbufReadU32(src); + gpsSolDRV.llh.lon = sbufReadU32(src); + gpsSolDRV.llh.alt = sbufReadU32(src); + gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); + gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); + + gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); + + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; ENABLE_STATE(GPS_FIX); } else { sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } // Feed data to navigation + gpsProcessNewDriverData(); gpsProcessNewSolutionData(); } else { sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index e75193617ab..10ade84f4df 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -82,7 +82,13 @@ typedef struct { // GPS public data gpsReceiverData_t gpsState; gpsStatistics_t gpsStats; -gpsSolutionData_t gpsSol; + +//it is not safe to access gpsSolDRV which is filled in gps thread by driver. +//gpsSolDRV can be accessed only after driver signalled that data is ready +//we copy gpsSolDRV to gpsSol, process by "Disable GPS logic condition" and "GPS Fix estimation" features +//and use it in the rest of code. +gpsSolutionData_t gpsSolDRV; //filled by driver +gpsSolutionData_t gpsSol; //used in the rest of the code // Map gpsBaudRate_e index to baudRate_e baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400 }; @@ -195,8 +201,6 @@ void processDisableGPSFix(void) gpsSol.llh.lat = last_lat; gpsSol.llh.lon = last_lon; gpsSol.llh.alt = last_alt; - - DISABLE_STATE(GPS_FIX); } else { last_lat = gpsSol.llh.lat; last_lon = gpsSol.llh.lon; @@ -204,6 +208,7 @@ void processDisableGPSFix(void) } } +//called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition" void updateEstimatedGPSFix(void) { static uint32_t lastUpdateMs = 0; @@ -215,16 +220,15 @@ void updateEstimatedGPSFix(void) int32_t dt = t - lastUpdateMs; lastUpdateMs = t; - if (STATE(GPS_FIX) || !canEstimateGPSFix()) { - DISABLE_STATE(GPS_ESTIMATED_FIX); + bool sensorHasFix = gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats; + + if (sensorHasFix || !canEstimateGPSFix()) { estimated_lat = gpsSol.llh.lat; estimated_lon = gpsSol.llh.lon; estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; return; } - ENABLE_STATE(GPS_ESTIMATED_FIX); - gpsSol.fixType = GPS_FIX_3D; gpsSol.hdop = 99; gpsSol.flags.hasNewData = true; @@ -283,28 +287,42 @@ void updateEstimatedGPSFix(void) } +void gpsProcessNewDriverData(void) +{ + gpsSol = gpsSolDRV; + + processDisableGPSFix(); + updateEstimatedGPSFix(); +} +//called after: +//1)driver copies gpsSolDRV to gpsSol +//2)gpsSol is processed by "Disable GPS logical switch" +//3)gpsSol is processed by GPS Fix estimator void gpsProcessNewSolutionData(void) { - // Set GPS fix flag only if we have 3D fix - if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { - ENABLE_STATE(GPS_FIX); - } - else { - /* When no fix available - reset flags as well */ - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - gpsSol.flags.validEPE = false; + if ( gpsSol.numSat == 99 ) { + ENABLE_STATE(GPS_ESTIMATED_FIX); DISABLE_STATE(GPS_FIX); + } else { + DISABLE_STATE(GPS_ESTIMATED_FIX); + + // Set GPS fix flag only if we have 3D fix + if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { + ENABLE_STATE(GPS_FIX); + } + else { + /* When no fix available - reset flags as well */ + gpsSol.flags.validVelNE = false; + gpsSol.flags.validVelD = false; + gpsSol.flags.validEPE = false; + DISABLE_STATE(GPS_FIX); + } } // Set sensor as ready and available sensorsSet(SENSOR_GPS); - processDisableGPSFix(); - - updateEstimatedGPSFix(); - // Pass on GPS update to NAV and IMU onNewGPSData(); @@ -322,29 +340,27 @@ void gpsProcessNewSolutionData(void) gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; } -static void gpsResetSolution(void) +static void gpsResetSolution(gpsSolutionData_t* gpsSol) { - gpsSol.eph = 9999; - gpsSol.epv = 9999; - gpsSol.numSat = 0; - gpsSol.hdop = 9999; + gpsSol->eph = 9999; + gpsSol->epv = 9999; + gpsSol->numSat = 0; + gpsSol->hdop = 9999; - gpsSol.fixType = GPS_NO_FIX; + gpsSol->fixType = GPS_NO_FIX; - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - gpsSol.flags.validMag = false; - gpsSol.flags.validEPE = false; - gpsSol.flags.validTime = false; + gpsSol->flags.validVelNE = false; + gpsSol->flags.validVelD = false; + gpsSol->flags.validEPE = false; + gpsSol->flags.validTime = false; } void gpsTryEstimateOnTimeout(void) { - gpsResetSolution(); + gpsResetSolution(&gpsSolDRV); + gpsResetSolution(&gpsSol); DISABLE_STATE(GPS_FIX); - processDisableGPSFix(); - updateEstimatedGPSFix(); if (STATE(GPS_ESTIMATED_FIX)) { @@ -369,7 +385,8 @@ void gpsInit(void) gpsStats.timeouts = 0; // Reset solution, timeout and prepare to start - gpsResetSolution(); + gpsResetSolution(&gpsSolDRV); + gpsResetSolution(&gpsSol); gpsSetProtocolTimeout(gpsState.baseTimeoutMs); gpsSetState(GPS_UNKNOWN); @@ -477,7 +494,8 @@ bool gpsUpdate(void) gpsSol.fixType = GPS_NO_FIX; // Reset solution - gpsResetSolution(); + gpsResetSolution(&gpsSolDRV); + gpsResetSolution(&gpsSol); // Call GPS protocol reset handler gpsProviders[gpsState.gpsConfig->provider].restart(); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 92241c95602..a83662588c8 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -118,7 +118,6 @@ typedef struct gpsSolutionData_s { bool gpsHeartbeat; // Toggle each update bool validVelNE; bool validVelD; - bool validMag; bool validEPE; // EPH/EPV values are valid - actual accuracy bool validTime; } flags; @@ -127,7 +126,6 @@ typedef struct gpsSolutionData_s { uint8_t numSat; gpsLocation_t llh; - int16_t magData[3]; int16_t velNED[3]; int16_t groundSpeed; diff --git a/src/main/io/gps_fake.c b/src/main/io/gps_fake.c index 6d574893436..30e8eeb94ab 100644 --- a/src/main/io/gps_fake.c +++ b/src/main/io/gps_fake.c @@ -62,37 +62,39 @@ void gpsFakeSet( int16_t velNED_Z, time_t time) { - gpsSol.fixType = fixType; - gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSol.numSat = numSat; + gpsSolDRV.fixType = fixType; + gpsSolDRV.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.numSat = numSat; - gpsSol.llh.lat = lat; - gpsSol.llh.lon = lon; - gpsSol.llh.alt = alt; - gpsSol.groundSpeed = groundSpeed; - gpsSol.groundCourse = groundCourse; - gpsSol.velNED[X] = velNED_X; - gpsSol.velNED[Y] = velNED_Y; - gpsSol.velNED[Z] = velNED_Z; - gpsSol.eph = 100; - gpsSol.epv = 100; - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - gpsSol.flags.hasNewData = true; + gpsSolDRV.llh.lat = lat; + gpsSolDRV.llh.lon = lon; + gpsSolDRV.llh.alt = alt; + gpsSolDRV.groundSpeed = groundSpeed; + gpsSolDRV.groundCourse = groundCourse; + gpsSolDRV.velNED[X] = velNED_X; + gpsSolDRV.velNED[Y] = velNED_Y; + gpsSolDRV.velNED[Z] = velNED_Z; + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.hasNewData = true; if (time) { struct tm* gTime = gmtime(&time); - gpsSol.time.year = (uint16_t)(gTime->tm_year + 1900); - gpsSol.time.month = (uint16_t)(gTime->tm_mon + 1); - gpsSol.time.day = (uint8_t)gTime->tm_mday; - gpsSol.time.hours = (uint8_t)gTime->tm_hour; - gpsSol.time.minutes = (uint8_t)gTime->tm_min; - gpsSol.time.seconds = (uint8_t)gTime->tm_sec; - gpsSol.time.millis = 0; - gpsSol.flags.validTime = gpsSol.fixType >= 3; + gpsSolDRV.time.year = (uint16_t)(gTime->tm_year + 1900); + gpsSolDRV.time.month = (uint16_t)(gTime->tm_mon + 1); + gpsSolDRV.time.day = (uint8_t)gTime->tm_mday; + gpsSolDRV.time.hours = (uint8_t)gTime->tm_hour; + gpsSolDRV.time.minutes = (uint8_t)gTime->tm_min; + gpsSolDRV.time.seconds = (uint8_t)gTime->tm_sec; + gpsSolDRV.time.millis = 0; + gpsSolDRV.flags.validTime = gpsSol.fixType >= 3; } + + gpsProcessNewDriverData(); } #endif diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index 8eafb2dff19..d4958117c92 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -81,33 +81,34 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr) { const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr; - gpsSol.fixType = gpsMapFixType(pkt->fixType); - gpsSol.numSat = pkt->satellitesInView; - gpsSol.llh.lon = pkt->longitude; - gpsSol.llh.lat = pkt->latitude; - gpsSol.llh.alt = pkt->mslAltitude; - gpsSol.velNED[X] = pkt->nedVelNorth; - gpsSol.velNED[Y] = pkt->nedVelEast; - gpsSol.velNED[Z] = pkt->nedVelDown; - gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast); - gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10 - gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); - gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); - gpsSol.hdop = gpsConstrainHDOP(pkt->hdop); - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - - gpsSol.time.year = pkt->year; - gpsSol.time.month = pkt->month; - gpsSol.time.day = pkt->day; - gpsSol.time.hours = pkt->hour; - gpsSol.time.minutes = pkt->min; - gpsSol.time.seconds = pkt->sec; - gpsSol.time.millis = 0; - - gpsSol.flags.validTime = (pkt->fixType >= 3); - + gpsSolDRV.fixType = gpsMapFixType(pkt->fixType); + gpsSolDRV.numSat = pkt->satellitesInView; + gpsSolDRV.llh.lon = pkt->longitude; + gpsSolDRV.llh.lat = pkt->latitude; + gpsSolDRV.llh.alt = pkt->mslAltitude; + gpsSolDRV.velNED[X] = pkt->nedVelNorth; + gpsSolDRV.velNED[Y] = pkt->nedVelEast; + gpsSolDRV.velNED[Z] = pkt->nedVelDown; + gpsSolDRV.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast); + gpsSolDRV.groundCourse = pkt->groundCourse / 10; // in deg * 10 + gpsSolDRV.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); + gpsSolDRV.hdop = gpsConstrainHDOP(pkt->hdop); + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + + gpsSolDRV.time.year = pkt->year; + gpsSolDRV.time.month = pkt->month; + gpsSolDRV.time.day = pkt->day; + gpsSolDRV.time.hours = pkt->hour; + gpsSolDRV.time.minutes = pkt->min; + gpsSolDRV.time.seconds = pkt->sec; + gpsSolDRV.time.millis = 0; + + gpsSolDRV.flags.validTime = (pkt->fixType >= 3); + + gpsProcessNewDriverData(); newDataReady = true; } #endif diff --git a/src/main/io/gps_nmea.c b/src/main/io/gps_nmea.c index c5c3b420db7..7269b102fbd 100755 --- a/src/main/io/gps_nmea.c +++ b/src/main/io/gps_nmea.c @@ -200,45 +200,45 @@ static bool gpsNewFrameNMEA(char c) switch (gps_frame) { case FRAME_GGA: frameOK = 1; - gpsSol.numSat = gps_Msg.numSat; + gpsSolDRV.numSat = gps_Msg.numSat; if (gps_Msg.fix) { - gpsSol.fixType = GPS_FIX_3D; // NMEA doesn't report fix type, assume 3D + gpsSolDRV.fixType = GPS_FIX_3D; // NMEA doesn't report fix type, assume 3D - gpsSol.llh.lat = gps_Msg.latitude; - gpsSol.llh.lon = gps_Msg.longitude; - gpsSol.llh.alt = gps_Msg.altitude; + gpsSolDRV.llh.lat = gps_Msg.latitude; + gpsSolDRV.llh.lon = gps_Msg.longitude; + gpsSolDRV.llh.alt = gps_Msg.altitude; // EPH/EPV are unreliable for NMEA as they are not real accuracy - gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop); - gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); - gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); - gpsSol.flags.validEPE = false; + gpsSolDRV.hdop = gpsConstrainHDOP(gps_Msg.hdop); + gpsSolDRV.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); + gpsSolDRV.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); + gpsSolDRV.flags.validEPE = false; } else { - gpsSol.fixType = GPS_NO_FIX; + gpsSolDRV.fixType = GPS_NO_FIX; } // NMEA does not report VELNED - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; + gpsSolDRV.flags.validVelNE = false; + gpsSolDRV.flags.validVelD = false; break; case FRAME_RMC: - gpsSol.groundSpeed = gps_Msg.speed; - gpsSol.groundCourse = gps_Msg.ground_course; + gpsSolDRV.groundSpeed = gps_Msg.speed; + gpsSolDRV.groundCourse = gps_Msg.ground_course; // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid if (gps_Msg.date != 0 && gps_Msg.time != 0) { - gpsSol.time.year = (gps_Msg.date % 100) + 2000; - gpsSol.time.month = (gps_Msg.date / 100) % 100; - gpsSol.time.day = (gps_Msg.date / 10000) % 100; - gpsSol.time.hours = (gps_Msg.time / 1000000) % 100; - gpsSol.time.minutes = (gps_Msg.time / 10000) % 100; - gpsSol.time.seconds = (gps_Msg.time / 100) % 100; - gpsSol.time.millis = (gps_Msg.time & 100) * 10; - gpsSol.flags.validTime = true; + gpsSolDRV.time.year = (gps_Msg.date % 100) + 2000; + gpsSolDRV.time.month = (gps_Msg.date / 100) % 100; + gpsSolDRV.time.day = (gps_Msg.date / 10000) % 100; + gpsSolDRV.time.hours = (gps_Msg.time / 1000000) % 100; + gpsSolDRV.time.minutes = (gps_Msg.time / 10000) % 100; + gpsSolDRV.time.seconds = (gps_Msg.time / 100) % 100; + gpsSolDRV.time.millis = (gps_Msg.time & 100) * 10; + gpsSolDRV.flags.validTime = true; } else { - gpsSol.flags.validTime = false; + gpsSolDRV.flags.validTime = false; } break; @@ -276,8 +276,9 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread) while (serialRxBytesWaiting(gpsState.gpsPort)) { uint8_t newChar = serialRead(gpsState.gpsPort); if (gpsNewFrameNMEA(newChar)) { - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; + gpsSolDRV.flags.validVelNE = false; + gpsSolDRV.flags.validVelD = false; + gpsProcessNewDriverData(); ptSemaphoreSignal(semNewDataReady); break; } diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 6e9abc72ffe..2a46c8670d2 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -57,6 +57,7 @@ typedef struct { } gpsReceiverData_t; extern gpsReceiverData_t gpsState; +extern gpsSolutionData_t gpsSolDRV; extern baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT]; @@ -66,6 +67,7 @@ extern void gpsFinalizeChangeBaud(void); extern uint16_t gpsConstrainEPE(uint32_t epe); extern uint16_t gpsConstrainHDOP(uint32_t hdop); +void gpsProcessNewDriverData(void); void gpsProcessNewSolutionData(void); void gpsSetProtocolTimeout(timeMs_t timeoutMs); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 330cb2b75fc..aba2d930f92 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -604,84 +604,84 @@ static bool gpsParceFrameUBLOX(void) { switch (_msg_id) { case MSG_POSLLH: - gpsSol.llh.lon = _buffer.posllh.longitude; - gpsSol.llh.lat = _buffer.posllh.latitude; - gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm - gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10); - gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10); - gpsSol.flags.validEPE = true; + gpsSolDRV.llh.lon = _buffer.posllh.longitude; + gpsSolDRV.llh.lat = _buffer.posllh.latitude; + gpsSolDRV.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm + gpsSolDRV.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10); + gpsSolDRV.flags.validEPE = true; if (next_fix_type != GPS_NO_FIX) - gpsSol.fixType = next_fix_type; + gpsSolDRV.fixType = next_fix_type; _new_position = true; break; case MSG_STATUS: next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type); if (next_fix_type == GPS_NO_FIX) - gpsSol.fixType = GPS_NO_FIX; + gpsSolDRV.fixType = GPS_NO_FIX; break; case MSG_SOL: next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type); if (next_fix_type == GPS_NO_FIX) - gpsSol.fixType = GPS_NO_FIX; - gpsSol.numSat = _buffer.solution.satellites; - gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP); + gpsSolDRV.fixType = GPS_NO_FIX; + gpsSolDRV.numSat = _buffer.solution.satellites; + gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP); break; case MSG_VELNED: - gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s - gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - gpsSol.velNED[X] = _buffer.velned.ned_north; - gpsSol.velNED[Y] = _buffer.velned.ned_east; - gpsSol.velNED[Z] = _buffer.velned.ned_down; - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; + gpsSolDRV.groundSpeed = _buffer.velned.speed_2d; // cm/s + gpsSolDRV.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + gpsSolDRV.velNED[X] = _buffer.velned.ned_north; + gpsSolDRV.velNED[Y] = _buffer.velned.ned_east; + gpsSolDRV.velNED[Z] = _buffer.velned.ned_down; + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; _new_speed = true; break; case MSG_TIMEUTC: if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) { - gpsSol.time.year = _buffer.timeutc.year; - gpsSol.time.month = _buffer.timeutc.month; - gpsSol.time.day = _buffer.timeutc.day; - gpsSol.time.hours = _buffer.timeutc.hour; - gpsSol.time.minutes = _buffer.timeutc.min; - gpsSol.time.seconds = _buffer.timeutc.sec; - gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000); - - gpsSol.flags.validTime = true; + gpsSolDRV.time.year = _buffer.timeutc.year; + gpsSolDRV.time.month = _buffer.timeutc.month; + gpsSolDRV.time.day = _buffer.timeutc.day; + gpsSolDRV.time.hours = _buffer.timeutc.hour; + gpsSolDRV.time.minutes = _buffer.timeutc.min; + gpsSolDRV.time.seconds = _buffer.timeutc.sec; + gpsSolDRV.time.millis = _buffer.timeutc.nano / (1000*1000); + + gpsSolDRV.flags.validTime = true; } else { - gpsSol.flags.validTime = false; + gpsSolDRV.flags.validTime = false; } break; case MSG_PVT: next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type); - gpsSol.fixType = next_fix_type; - gpsSol.llh.lon = _buffer.pvt.longitude; - gpsSol.llh.lat = _buffer.pvt.latitude; - gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm - gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s - gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s - gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s - gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s - gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - gpsSol.numSat = _buffer.pvt.satellites; - gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10); - gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10); - gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP); - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; + gpsSolDRV.fixType = next_fix_type; + gpsSolDRV.llh.lon = _buffer.pvt.longitude; + gpsSolDRV.llh.lat = _buffer.pvt.latitude; + gpsSolDRV.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm + gpsSolDRV.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s + gpsSolDRV.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s + gpsSolDRV.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s + gpsSolDRV.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s + gpsSolDRV.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + gpsSolDRV.numSat = _buffer.pvt.satellites; + gpsSolDRV.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10); + gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP); + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) { - gpsSol.time.year = _buffer.pvt.year; - gpsSol.time.month = _buffer.pvt.month; - gpsSol.time.day = _buffer.pvt.day; - gpsSol.time.hours = _buffer.pvt.hour; - gpsSol.time.minutes = _buffer.pvt.min; - gpsSol.time.seconds = _buffer.pvt.sec; - gpsSol.time.millis = _buffer.pvt.nano / (1000*1000); - - gpsSol.flags.validTime = true; + gpsSolDRV.time.year = _buffer.pvt.year; + gpsSolDRV.time.month = _buffer.pvt.month; + gpsSolDRV.time.day = _buffer.pvt.day; + gpsSolDRV.time.hours = _buffer.pvt.hour; + gpsSolDRV.time.minutes = _buffer.pvt.min; + gpsSolDRV.time.seconds = _buffer.pvt.sec; + gpsSolDRV.time.millis = _buffer.pvt.nano / (1000*1000); + + gpsSolDRV.flags.validTime = true; } else { - gpsSol.flags.validTime = false; + gpsSolDRV.flags.validTime = false; } _new_position = true; @@ -985,6 +985,7 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread) while (serialRxBytesWaiting(gpsState.gpsPort)) { uint8_t newChar = serialRead(gpsState.gpsPort); if (gpsNewFrameUBLOX(newChar)) { + gpsProcessNewDriverData(); ptSemaphoreSignal(semNewDataReady); break; } From 8c933edb2ca0b1e16466f6c95fe080b52811fffa Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 28 Apr 2023 22:33:18 +0200 Subject: [PATCH 037/199] fixed HITL HW sensors failure simulation --- src/main/fc/fc_msp.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 432ad6e6a7a..828fbe63ee8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3595,8 +3595,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #endif if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - simulatorData.airSpeed = sbufReadU16(src); - } + simulatorData.airSpeed = sbufReadU16(src); + } else { + if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { + sbufReadU16(src); + } + } if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; From 7d007a5c191c428ef8ab851c5399e8d05e825d35 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 21:14:34 +0200 Subject: [PATCH 038/199] use wind vector for course correction on estimation on gps timeout --- src/main/io/gps.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 10ade84f4df..6b94c12ee0a 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -574,10 +574,9 @@ bool isGPSHealthy(void) return true; } -//NOTE: checks if real GPS data present, ignoring any available GPS Fix estimation bool isGPSHeadingValid(void) { - return sensors(SENSOR_GPS) && ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; + return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; } #endif From cdd299f045ef0d520f85c55027f40ca4735fb19e Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 23:40:06 +0200 Subject: [PATCH 039/199] fixed gps fix estimation on gps timeout --- src/main/fc/fc_msp.c | 5 +---- src/main/io/gps.c | 43 ++++++++++++++++----------------------- src/main/io/gps_fake.c | 3 +-- src/main/io/gps_msp.c | 2 +- src/main/io/gps_nmea.c | 2 +- src/main/io/gps_private.h | 2 +- src/main/io/gps_ublox.c | 2 +- 7 files changed, 24 insertions(+), 35 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 828fbe63ee8..efec7bf6440 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3515,7 +3515,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { gpsSolDRV.fixType = sbufReadU8(src); gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSolDRV.flags.hasNewData = true; gpsSolDRV.numSat = sbufReadU8(src); if (gpsSolDRV.fixType != GPS_NO_FIX) { @@ -3536,14 +3535,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu gpsSolDRV.eph = 100; gpsSolDRV.epv = 100; - - ENABLE_STATE(GPS_FIX); } else { sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } // Feed data to navigation gpsProcessNewDriverData(); - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } else { sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 6b94c12ee0a..2d2f2c506b1 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -168,7 +168,7 @@ bool canEstimateGPSFix(void) #if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: - //1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once + //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_BARO) && baroIsHealthy() && @@ -231,7 +231,6 @@ void updateEstimatedGPSFix(void) gpsSol.fixType = GPS_FIX_3D; gpsSol.hdop = 99; - gpsSol.flags.hasNewData = true; gpsSol.numSat = 99; gpsSol.eph = 100; @@ -298,8 +297,9 @@ void gpsProcessNewDriverData(void) //called after: //1)driver copies gpsSolDRV to gpsSol //2)gpsSol is processed by "Disable GPS logical switch" -//3)gpsSol is processed by GPS Fix estimator -void gpsProcessNewSolutionData(void) +//3)gpsSol is processed by GPS Fix estimator - updateEstimatedGPSFix() +//On GPS sensor timeout - called after updateEstimatedGPSFix() +void gpsProcessNewSolutionData(bool timeout) { if ( gpsSol.numSat == 99 ) { ENABLE_STATE(GPS_ESTIMATED_FIX); @@ -320,8 +320,10 @@ void gpsProcessNewSolutionData(void) } } - // Set sensor as ready and available - sensorsSet(SENSOR_GPS); + if (!timeout) { + // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix) + sensorsSet(SENSOR_GPS); + } // Pass on GPS update to NAV and IMU onNewGPSData(); @@ -357,15 +359,13 @@ static void gpsResetSolution(gpsSolutionData_t* gpsSol) void gpsTryEstimateOnTimeout(void) { - gpsResetSolution(&gpsSolDRV); gpsResetSolution(&gpsSol); DISABLE_STATE(GPS_FIX); updateEstimatedGPSFix(); - if (STATE(GPS_ESTIMATED_FIX)) { - onNewGPSData(); - gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; + if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in + gpsProcessNewSolutionData(true); } } @@ -462,28 +462,21 @@ bool gpsUpdate(void) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) - { + if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) { gpsSetState(GPS_LOST_COMMUNICATION); sensorsClear(SENSOR_GPS); gpsStats.timeouts = 5; gpsTryEstimateOnTimeout(); - return false; - } - else - { + } else { gpsSetState(GPS_RUNNING); sensorsSet(SENSOR_GPS); - bool res = gpsSol.flags.hasNewData; - gpsSol.flags.hasNewData = false; - return res; } + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; } #endif - // Assume that we don't have new data this run - gpsSol.flags.hasNewData = false; - switch (gpsState.state) { default: case GPS_INITIALIZING: @@ -491,11 +484,9 @@ bool gpsUpdate(void) if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) { // Reset internals DISABLE_STATE(GPS_FIX); - gpsSol.fixType = GPS_NO_FIX; // Reset solution gpsResetSolution(&gpsSolDRV); - gpsResetSolution(&gpsSol); // Call GPS protocol reset handler gpsProviders[gpsState.gpsConfig->provider].restart(); @@ -529,7 +520,9 @@ bool gpsUpdate(void) gpsTryEstimateOnTimeout(); } - return gpsSol.flags.hasNewData; + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; } void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) diff --git a/src/main/io/gps_fake.c b/src/main/io/gps_fake.c index 30e8eeb94ab..8471800a0e4 100644 --- a/src/main/io/gps_fake.c +++ b/src/main/io/gps_fake.c @@ -46,7 +46,7 @@ void gpsFakeRestart(void) void gpsFakeHandle(void) { - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } void gpsFakeSet( @@ -79,7 +79,6 @@ void gpsFakeSet( gpsSolDRV.flags.validVelNE = true; gpsSolDRV.flags.validVelD = true; gpsSolDRV.flags.validEPE = true; - gpsSolDRV.flags.hasNewData = true; if (time) { struct tm* gTime = gmtime(&time); diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index d4958117c92..1fd4b1cf15e 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -63,7 +63,7 @@ void gpsRestartMSP(void) void gpsHandleMSP(void) { if (newDataReady) { - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); newDataReady = false; } } diff --git a/src/main/io/gps_nmea.c b/src/main/io/gps_nmea.c index 7269b102fbd..be27d22ab54 100755 --- a/src/main/io/gps_nmea.c +++ b/src/main/io/gps_nmea.c @@ -313,7 +313,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThreadNMEA) // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore while (1) { ptSemaphoreWait(semNewDataReady); - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } ptEnd(0); diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 2a46c8670d2..8c0d8a3ec4e 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -68,7 +68,7 @@ extern uint16_t gpsConstrainEPE(uint32_t epe); extern uint16_t gpsConstrainHDOP(uint32_t hdop); void gpsProcessNewDriverData(void); -void gpsProcessNewSolutionData(void); +void gpsProcessNewSolutionData(bool); void gpsSetProtocolTimeout(timeMs_t timeoutMs); extern void gpsRestartUBLOX(void); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index aba2d930f92..8f5be8186ff 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1053,7 +1053,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore while (1) { ptSemaphoreWait(semNewDataReady); - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } ptEnd(0); From 3040cd727b0445d2e00bb674a5543d6d83192e51 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 23:40:34 +0200 Subject: [PATCH 040/199] return 99 satellites for logic condition on estimation --- src/main/programming/logic_condition.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index cbcbfa57385..f4600b73511 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -663,7 +663,9 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: - if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + if ( STATE(GPS_ESTIMATED_FIX) ){ + return gpsSol.numSat; //99 + } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { return 0; } else { return gpsSol.numSat; From daac5b5df36fd64fc7399cb5890a3dd9bbbd9a03 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 23:40:55 +0200 Subject: [PATCH 041/199] removed redundand code --- src/main/io/osd.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index bbf02b359d1..1f5333861ee 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1728,9 +1728,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); if (STATE(GPS_ESTIMATED_FIX)) { - if (!STATE(GPS_FIX)) { - strcpy(buff + 2, "ES"); - } + strcpy(buff + 2, "ES"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { From f18b5a0fc5cd1b8cfb1b2b493c5cd24dda46d7db Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 19 May 2023 23:41:18 +0200 Subject: [PATCH 042/199] allow course correction on estimation with gps sensor timeout --- src/main/flight/imu.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 1c9feddf144..0fd56f82901 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -323,10 +323,9 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c #endif } -//NOTE: checks if real GPS data is present, ignores any available GPS Fix estimation bool isGPSTrustworthy(void) { - return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6; + return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX); } static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) From 7e777faacb15c19a9be73567a702c1a3f68fa4a3 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 20 May 2023 01:21:57 +0300 Subject: [PATCH 043/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 4f43eed353d..0bf53fa063c 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -19,22 +19,21 @@ By befault, all navigation modes are disabled when GPS fix is lost. If RC signal GPS fix estimation allows to recover plane using magnetometer and baromener only. -Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. -GPS Fix is also estimated on GPS Sensor timeout (harware failure). +GPS Fix is also estimated on GPS Sensor timeouts (hardware failures). # How it works ? In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints. - -Without GPS fix, plane has nose heading from magnetometer only. +Without GPS fix, plane has nose heading from magnetometer and height from barometer only. To navigate without GPS fix, we make the following assumptions: - plane is flying in the direction where nose is pointing - (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings -It is posible to roughtly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). +It is posible to roughtly estimate position using these assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. @@ -49,7 +48,7 @@ GPS Fix estimation is enabled with CLI command: ```set inav_allow_gps_fix_estimation=ON``` -Also you have to specify criuse airspeed of the plane. +Also you have to specify cruise airspeed of the plane. To find out cruise airspeed, make a test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. @@ -64,7 +63,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 cm/s *It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Increase cruise speed with throttle" - do not use it without GPS Fix.* -*If pitot is available, pitot sensor data will be used instead of constant.* +*If pitot is available, pitot sensor data will be used instead of constant. It is not necessary to specify fw_reference_airspeed. However, it is still adviced to specify for the case of pitot failure.* *Note related command: to continue mission without RC signal, see command ```set failsafe_mission_delay=-1```.* @@ -84,7 +83,19 @@ For testing purpoces, it is possible to disable GPS sensor fix from RC controlle ```failsafe_gps_fix_estimation_delay``` -Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. +Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. RTH is trigerred regradless of failsafe procedure selected in configurator. + +# Expected error + +Relistic expected error is up to 20%. In tests, 500m drift per 5km path was seen. + +To dicrease drift: +- fly one large circle with GPS available to get good wind estimation +- use airspeed sensor +- do large turns +- make sure compass is pointing nose direction precicely +- calibrate compass correctly + # Is it possible to implement this for multirotor ? From ff1cd25a163743eaf3821b9b5644a35ed2d55b13 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 20 May 2023 13:33:59 +0200 Subject: [PATCH 044/199] fixed compilation error --- src/main/io/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 2d2f2c506b1..5af00c8dcd0 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -259,7 +259,7 @@ void updateEstimatedGPSFix(void) } // here (velX, velY) is estimated horizontal speed with wind influence = ground speed - if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent() == 0))) { + if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent(false) == 0))) { velX = 0; velY = 0; } From 8778980868e737c64530b0dc48e765fcbefbe6df Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 7 Jul 2023 13:07:21 +0300 Subject: [PATCH 045/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 0bf53fa063c..533b0e2da28 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -87,15 +87,23 @@ Controls whether waypoint mission is allowed to proceed with gps fix estimation. # Expected error -Relistic expected error is up to 20%. In tests, 500m drift per 5km path was seen. +Realistic expected error is up to 200m per 1km of flight path. In tests, 500m drift per 5km path was seen. To dicrease drift: - fly one large circle with GPS available to get good wind estimation -- use airspeed sensor -- do large turns -- make sure compass is pointing nose direction precicely +- use airspeed sensor. If airspeed sensor is not installed, fly in cruise mode without throttle override. +- do smooth, large turns +- make sure compass is pointing in nose direction precicely - calibrate compass correctly +This video shown real world test where GPS was disabled occasionally. Wind is 10km/h south-west: + + +https://github.com/RomanLut/inav/assets/11955117/0599a3c3-df06-4d40-a32a-4d8f96140592 + + +Purple line shows estimated position. Black line shows real position. "EST ERR" sensor shows estimation error in metters. Estimation is running when satellite icon displays "ES". Estimated position snaps to real position when GPS fix is reaquired. + # Is it possible to implement this for multirotor ? From 5eb46f5328759f522011b17a06161a39efdd4246 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 4 Aug 2023 03:32:55 +0300 Subject: [PATCH 046/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 533b0e2da28..04fe34ea7f5 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -15,7 +15,7 @@ Plane should have the following sensors: - magnethometer - pitot (optional) -By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above inreachable spaces, plane will be lost. +By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above unreachable spaces, plane will be lost. GPS fix estimation allows to recover plane using magnetometer and baromener only. From 8b1330c4388d1e60eed645bdfa87d4e73dc15b70 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 4 Aug 2023 03:34:29 +0300 Subject: [PATCH 047/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 04fe34ea7f5..dd7a4de55fb 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -33,7 +33,7 @@ To navigate without GPS fix, we make the following assumptions: - plane is flying in the direction where nose is pointing - (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings -It is posible to roughtly estimate position using these assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). +It is possible to roughly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. From ee66d0f6194baeb37f958bd083157e1340997fee Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 4 Aug 2023 03:35:18 +0300 Subject: [PATCH 048/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index dd7a4de55fb..d5bb7c7465b 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -35,9 +35,9 @@ To navigate without GPS fix, we make the following assumptions: It is possible to roughly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). -From estimated heading direction and speed, plane is able to **roughtly** estimate it's position. +From estimated heading direction and speed, plane is able to **roughty** estimate it's position. -It is assumed, that plane will fly in roughtly estimated direction to home position untill either GPS fix or RC signal is recovered. +It is assumed, that plane will fly in roughly estimated direction to home position untill either GPS fix or RC signal is recovered. *Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. From ae5c14409fe8e173ce59dbe0edb8bc2efecdb3c3 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 4 Aug 2023 03:37:37 +0300 Subject: [PATCH 049/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index d5bb7c7465b..4ed5365e301 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -96,7 +96,7 @@ To dicrease drift: - make sure compass is pointing in nose direction precicely - calibrate compass correctly -This video shown real world test where GPS was disabled occasionally. Wind is 10km/h south-west: +This video shows real world test where GPS was disabled occasionally. Wind is 10km/h south-west: https://github.com/RomanLut/inav/assets/11955117/0599a3c3-df06-4d40-a32a-4d8f96140592 From f2108d74c41d7ff1f2323ab71d9aec91f01b9aa7 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 7 Aug 2023 14:36:50 +0900 Subject: [PATCH 050/199] back calculation anti-windup can only shrink integrator --- src/main/common/fp_pid.c | 7 ++++++- src/main/common/maths.h | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c index 5edaa193b8c..df8ccaad6b7 100644 --- a/src/main/common/fp_pid.c +++ b/src/main/common/fp_pid.c @@ -92,6 +92,11 @@ float navPidApply3( /* Pre-calculate output and limit it if actuator is saturating */ const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward; const float outValConstrained = constrainf(outVal, outMin, outMax); + float backCalc = outValConstrained - outVal;//back-calculation anti-windup + if (SIGN(backCalc) == SIGN(pid->integrator)) { + //back calculation anti-windup can only shrink integrator, will not push it to the opposite direction + backCalc = 0.0f; + } pid->proportional = newProportional; pid->integral = pid->integrator; @@ -104,7 +109,7 @@ float navPidApply3( !(pidFlags & PID_ZERO_INTEGRATOR) && !(pidFlags & PID_FREEZE_INTEGRATOR) ) { - const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt); + const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + (backCalc * pid->param.kT * dt); if (pidFlags & PID_SHRINK_INTEGRATOR) { // Only allow integrator to shrink diff --git a/src/main/common/maths.h b/src/main/common/maths.h index cc3d1bc5176..5e00eb0c7c7 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -91,6 +91,7 @@ ) #define MIN(a, b) _CHOOSE(<, a, b) #define MAX(a, b) _CHOOSE(>, a, b) +#define SIGN(a) ((a > 0) ? 1 : -1) #define _ABS_II(x, var) \ ( __extension__ ({ \ From d2afb64f2ecd9b21ef52d6df7fddbbc22e00e91b Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 7 Aug 2023 17:51:16 +0900 Subject: [PATCH 051/199] minor fix --- src/main/common/maths.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 5e00eb0c7c7..fc8dbe33295 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -91,7 +91,7 @@ ) #define MIN(a, b) _CHOOSE(<, a, b) #define MAX(a, b) _CHOOSE(>, a, b) -#define SIGN(a) ((a > 0) ? 1 : -1) +#define SIGN(a) ((a >= 0) ? 1 : -1) #define _ABS_II(x, var) \ ( __extension__ ({ \ From 599e45c48f2eb8cd91c462712b9eecdfbe05c84b Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 9 Aug 2023 09:48:46 +0200 Subject: [PATCH 052/199] added USE_GPS_FIX_ESTIMATION fixed indentation --- src/main/fc/fc_msp.c | 186 ++++---- src/main/fc/runtime_config.h | 20 +- src/main/fc/settings.yaml | 2 + src/main/flight/failsafe.c | 23 +- src/main/flight/failsafe.h | 4 + src/main/flight/imu.c | 6 +- src/main/flight/wind_estimator.c | 13 +- src/main/io/gps.c | 34 +- src/main/io/gps_ublox.c | 52 +-- src/main/io/osd.c | 398 ++++++++++-------- src/main/io/osd_dji_hd.c | 6 +- src/main/navigation/navigation.c | 8 +- src/main/navigation/navigation.h | 2 + src/main/navigation/navigation_fixedwing.c | 10 +- .../navigation/navigation_pos_estimator.c | 35 +- src/main/programming/logic_condition.c | 13 +- src/main/programming/logic_condition.h | 4 + src/main/sensors/diagnostics.c | 38 +- src/main/sensors/pitotmeter.c | 8 +- src/main/target/common.h | 1 + src/main/telemetry/ghst.c | 6 +- src/main/telemetry/hott.c | 12 +- src/main/telemetry/ibus_shared.c | 96 ++++- src/main/telemetry/jetiexbus.c | 6 +- src/main/telemetry/ltm.c | 6 +- src/main/telemetry/mavlink.c | 12 +- src/main/telemetry/sim.c | 6 +- src/main/telemetry/smartport.c | 6 +- src/main/telemetry/srxl.c | 12 +- 29 files changed, 627 insertions(+), 398 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7ea2ff20ebb..b4e22087453 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3492,8 +3492,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) { - if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once - DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once + DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); #ifdef USE_BARO if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { @@ -3508,15 +3508,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu // Review: Many states were affected. Reboot? disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! - } + } } else { - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once + if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once #ifdef USE_BARO if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { sensorsSet(SENSOR_BARO); setTaskEnabled(TASK_BARO, true); DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); - baroStartCalibration(); + baroStartCalibration(); } #endif @@ -3530,80 +3530,80 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu mag.magADC[Z] = 0; } #endif - ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); LOG_DEBUG(SYSTEM, "Simulator enabled"); } - if (dataSize >= 14) { - - if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { - gpsSolDRV.fixType = sbufReadU8(src); - gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSolDRV.numSat = sbufReadU8(src); - - if (gpsSolDRV.fixType != GPS_NO_FIX) { - gpsSolDRV.flags.validVelNE = true; - gpsSolDRV.flags.validVelD = true; - gpsSolDRV.flags.validEPE = true; - gpsSolDRV.flags.validTime = false; - - gpsSolDRV.llh.lat = sbufReadU32(src); - gpsSolDRV.llh.lon = sbufReadU32(src); - gpsSolDRV.llh.alt = sbufReadU32(src); - gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); - gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); - - gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); - - gpsSolDRV.eph = 100; - gpsSolDRV.epv = 100; - } else { - sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); - } + if (dataSize >= 14) { + + if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { + gpsSolDRV.fixType = sbufReadU8(src); + gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.numSat = sbufReadU8(src); + + if (gpsSolDRV.fixType != GPS_NO_FIX) { + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.validTime = false; + + gpsSolDRV.llh.lat = sbufReadU32(src); + gpsSolDRV.llh.lon = sbufReadU32(src); + gpsSolDRV.llh.alt = sbufReadU32(src); + gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); + gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); + + gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); + + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; + } else { + sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); + } // Feed data to navigation - gpsProcessNewDriverData(); - gpsProcessNewSolutionData(false); - } else { - sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); - } + gpsProcessNewDriverData(); + gpsProcessNewSolutionData(false); + } else { + sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); + } - if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) { - attitude.values.roll = (int16_t)sbufReadU16(src); - attitude.values.pitch = (int16_t)sbufReadU16(src); - attitude.values.yaw = (int16_t)sbufReadU16(src); - } else { - sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); - } + if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) { + attitude.values.roll = (int16_t)sbufReadU16(src); + attitude.values.pitch = (int16_t)sbufReadU16(src); + attitude.values.yaw = (int16_t)sbufReadU16(src); + } else { + sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); + } // Get the acceleration in 1G units - acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accVibeSq[X] = 0.0f; - acc.accVibeSq[Y] = 0.0f; - acc.accVibeSq[Z] = 0.0f; + acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accVibeSq[X] = 0.0f; + acc.accVibeSq[Y] = 0.0f; + acc.accVibeSq[Z] = 0.0f; // Get the angular velocity in DPS - gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; - gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; - gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; - - if (sensors(SENSOR_BARO)) { - baro.baroPressure = (int32_t)sbufReadU32(src); - baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); - } else { - sbufAdvance(src, sizeof(uint32_t)); - } + gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; - if (sensors(SENSOR_MAG)) { - mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT - mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero - mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; - } else { - sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); - } + if (sensors(SENSOR_BARO)) { + baro.baroPressure = (int32_t)sbufReadU32(src); + baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); + } else { + sbufAdvance(src, sizeof(uint32_t)); + } + + if (sensors(SENSOR_MAG)) { + mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT + mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero + mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; + } else { + sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); + } #if defined(USE_FAKE_BATT_SENSOR) if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { @@ -3617,7 +3617,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { simulatorData.airSpeed = sbufReadU16(src); - } else { + } else { if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { sbufReadU16(src); } @@ -3626,35 +3626,35 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; } - } else { - DISABLE_STATE(GPS_FIX); - } - } + } else { + DISABLE_STATE(GPS_FIX); + } + } - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]); - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]); - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]); - sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500)); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]); + sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500)); - simulatorData.debugIndex++; - if (simulatorData.debugIndex == 8) { - simulatorData.debugIndex = 0; - } + simulatorData.debugIndex++; + if (simulatorData.debugIndex == 8) { + simulatorData.debugIndex = 0; + } - tmp_u8 = simulatorData.debugIndex | - ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | - (ARMING_FLAG(ARMED) ? 64 : 0) | - (!feature(FEATURE_OSD) ? 32: 0) | - (!isOSDTypeSupportedBySimulator() ? 16 : 0); + tmp_u8 = simulatorData.debugIndex | + ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | + (ARMING_FLAG(ARMED) ? 64 : 0) | + (!feature(FEATURE_OSD) ? 32: 0) | + (!isOSDTypeSupportedBySimulator() ? 16 : 0); - sbufWriteU8(dst, tmp_u8); - sbufWriteU32(dst, debug[simulatorData.debugIndex]); + sbufWriteU8(dst, tmp_u8); + sbufWriteU32(dst, debug[simulatorData.debugIndex]); - sbufWriteU16(dst, attitude.values.roll); - sbufWriteU16(dst, attitude.values.pitch); - sbufWriteU16(dst, attitude.values.yaw); + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, attitude.values.yaw); - mspWriteSimulatorOSD(dst); + mspWriteSimulatorOSD(dst); *ret = MSP_RESULT_ACK; break; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 3b55a4b2816..48cf080ddd8 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -123,7 +123,9 @@ typedef enum { NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available COMPASS_CALIBRATED = (1 << 8), ACCELEROMETER_CALIBRATED = (1 << 9), +#ifdef USE_GPS_FIX_ESTIMATION GPS_ESTIMATED_FIX = (1 << 10), +#endif NAV_CRUISE_BRAKING = (1 << 11), NAV_CRUISE_BRAKING_BOOST = (1 << 12), NAV_CRUISE_BRAKING_LOCKED = (1 << 13), @@ -176,12 +178,12 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void); typedef enum { HITL_RESET_FLAGS = (0 << 0), - HITL_ENABLE = (1 << 0), - HITL_SIMULATE_BATTERY = (1 << 1), - HITL_MUTE_BEEPER = (1 << 2), - HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) - HITL_HAS_NEW_GPS_DATA = (1 << 4), - HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 + HITL_ENABLE = (1 << 0), + HITL_SIMULATE_BATTERY = (1 << 1), + HITL_MUTE_BEEPER = (1 << 2), + HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) + HITL_HAS_NEW_GPS_DATA = (1 << 4), + HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 HITL_AIRSPEED = (1 << 6), HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2 HITL_GPS_TIMEOUT = (1 << 8), @@ -189,10 +191,10 @@ typedef enum { } simulatorFlags_t; typedef struct { - simulatorFlags_t flags; - uint8_t debugIndex; + simulatorFlags_t flags; + uint8_t debugIndex; uint8_t vbat; // 126 -> 12.6V - uint16_t airSpeed; // cm/s + uint16_t airSpeed; // cm/s int16_t input[4]; } simulatorData_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fb25aeee7c1..b1e4d3f603b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -834,6 +834,7 @@ groups: max: 600 - name: failsafe_gps_fix_estimation_delay description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation." + condition: USE_GPS_FIX_ESTIMATION default_value: 7 min: -1 max: 600 @@ -2195,6 +2196,7 @@ groups: type: bool - name: inav_allow_gps_fix_estimation description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay." + condition: USE_GPS_FIX_ESTIMATION default_value: OFF field: allow_gps_fix_estimation type: bool diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index e2cd71cc9d7..d1d1ede6282 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -82,7 +82,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s) +#ifdef USE_GPS_FIX_ESTIMATION .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied +#endif ); typedef enum { @@ -355,7 +357,11 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set if (failsafeConfig()->failsafe_min_distance > 0 && - ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { + ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && STATE(GPS_FIX_HOME)) { // get the distance to the original arming point uint32_t distance = calculateDistanceToDestination(&original_rth_home); @@ -368,7 +374,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) return failsafeConfig()->failsafe_procedure; } - +#ifdef USE_GPS_FIX_ESTIMATION bool checkGPSFixFailsafe(void) { if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { @@ -387,6 +393,7 @@ bool checkGPSFixFailsafe(void) } return false; } +#endif void failsafeUpdateState(void) @@ -418,9 +425,12 @@ void failsafeUpdateState(void) failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } +#ifdef USE_GPS_FIX_ESTIMATION if ( checkGPSFixFailsafe() ) { reprocessState = true; - } else if (!receivingRxDataAndNotFailsafeMode) { + } else +#endif + if (!receivingRxDataAndNotFailsafeMode) { if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero @@ -432,7 +442,7 @@ void failsafeUpdateState(void) failsafeState.wpModeDelayedFailsafeStart = 0; } reprocessState = true; - } + } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (!receivingRxDataAndNotFailsafeMode) { @@ -488,11 +498,14 @@ void failsafeUpdateState(void) } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; reprocessState = true; - } else { + } +#ifdef USE_GPS_FIX_ESTIMATION + else { if ( checkGPSFixFailsafe() ) { reprocessState = true; } } +#endif break; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index d80ab650c78..459a201d242 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -43,7 +43,9 @@ typedef struct failsafeConfig_s { uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) +#ifdef USE_GPS_FIX_ESTIMATION int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) +#endif } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); @@ -150,7 +152,9 @@ typedef struct failsafeState_s { timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time +#ifdef USE_GPS_FIX_ESTIMATION timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation +#endif failsafeProcedure_e activeProcedure; failsafePhase_e phase; failsafeRxLinkState_e rxLinkState; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index b46224063f9..94223f04492 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -323,7 +323,11 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c bool isGPSTrustworthy(void) { - return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX); + return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ; } static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index b6174611add..1cf3b8941ee 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -52,7 +52,11 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT]; bool isEstimatedWindSpeedValid(void) { - return hasValidWindEstimate || STATE(GPS_ESTIMATED_FIX); //use any wind estimate with GPS fix estimation. + return hasValidWindEstimate +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) //use any wind estimate with GPS fix estimation. +#endif + ; } float getEstimatedWindSpeed(int axis) @@ -90,8 +94,11 @@ void updateWindEstimator(timeUs_t currentTimeUs) if (!STATE(FIXED_WING_LEGACY) || !isGPSHeadingValid() || !gpsSol.flags.validVelNE || - !gpsSol.flags.validVelD || - STATE(GPS_ESTIMATED_FIX)) { + !gpsSol.flags.validVelD +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { return; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 965b89531a8..0e5d166c7c7 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -223,7 +223,7 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) gpsState.timeoutMs = timeoutMs; } - +#ifdef USE_GPS_FIX_ESTIMATION bool canEstimateGPSFix(void) { #if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) @@ -238,10 +238,11 @@ bool canEstimateGPSFix(void) #else return false; -#endif - +#endif } +#endif +#ifdef USE_GPS_FIX_ESTIMATION void processDisableGPSFix(void) { static int32_t last_lat = 0; @@ -268,7 +269,9 @@ void processDisableGPSFix(void) last_alt = gpsSol.llh.alt; } } +#endif +#ifdef USE_GPS_FIX_ESTIMATION //called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition" void updateEstimatedGPSFix(void) { @@ -345,14 +348,17 @@ void updateEstimatedGPSFix(void) gpsSol.velNED[Y] = (int16_t)(velY); gpsSol.velNED[Z] = 0; } +#endif void gpsProcessNewDriverData(void) { gpsSol = gpsSolDRV; +#ifdef USE_GPS_FIX_ESTIMATION processDisableGPSFix(); updateEstimatedGPSFix(); +#endif } //called after: @@ -362,11 +368,13 @@ void gpsProcessNewDriverData(void) //On GPS sensor timeout - called after updateEstimatedGPSFix() void gpsProcessNewSolutionData(bool timeout) { +#ifdef USE_GPS_FIX_ESTIMATION if ( gpsSol.numSat == 99 ) { ENABLE_STATE(GPS_ESTIMATED_FIX); DISABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_ESTIMATED_FIX); +#endif // Set GPS fix flag only if we have 3D fix if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { @@ -379,7 +387,9 @@ void gpsProcessNewSolutionData(bool timeout) gpsSol.flags.validEPE = false; DISABLE_STATE(GPS_FIX); } +#ifdef USE_GPS_FIX_ESTIMATION } +#endif if (!timeout) { // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix) @@ -423,11 +433,15 @@ void gpsTryEstimateOnTimeout(void) gpsResetSolution(&gpsSol); DISABLE_STATE(GPS_FIX); - updateEstimatedGPSFix(); +#ifdef USE_GPS_FIX_ESTIMATION + if ( canEstimateGPSFix() ) { + updateEstimatedGPSFix(); - if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in - gpsProcessNewSolutionData(true); + if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in + gpsProcessNewSolutionData(true); + } } +#endif } void gpsPreInit(void) @@ -577,7 +591,7 @@ bool gpsUpdate(void) break; } - if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) { + if ( !sensors(SENSOR_GPS) ) { gpsTryEstimateOnTimeout(); } @@ -630,7 +644,11 @@ bool isGPSHealthy(void) bool isGPSHeadingValid(void) { - return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; + return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && gpsSol.groundSpeed >= 300; } #endif diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 10f1e6d9473..59eb68bd93b 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -147,7 +147,7 @@ static union { bool gpsUbloxHasGalileo(void) { return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK); - } +} bool gpsUbloxHasBeidou(void) { @@ -622,8 +622,8 @@ static bool gpsParseFrameUBLOX(void) gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { if (_buffer.ver.swVersion[9] > '2' || true) { - // check extensions; - // after hw + sw vers; each is 30 bytes + // check extensions; + // after hw + sw vers; each is 30 bytes bool found = false; for (int j = 40; j < _payload_length && !found; j += 30) { @@ -863,8 +863,8 @@ STATIC_PROTOTHREAD(gpsConfigure) if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error configureRATE(hz2rate(5)); // 5Hz - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } } else { // u-Blox 5/6/7/8 or unknown @@ -901,8 +901,8 @@ STATIC_PROTOTHREAD(gpsConfigure) if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error configureRATE(hz2rate(5)); // 5Hz - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } } // u-Blox 5/6 doesn't support PVT, use legacy config // UNKNOWN also falls here, use as a last resort @@ -944,19 +944,19 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure GNSS for M8N and later if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { - gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { + gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { configureGNSS10(); - } else { - configureGNSS(); - } - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } else { + configureGNSS(); + } + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - if(_ack_state == UBX_ACK_GOT_NAK) { + if(_ack_state == UBX_ACK_GOT_NAK) { gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT; gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT; - } + } } ptEnd(0); @@ -1019,18 +1019,18 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); } - // Reset protocol timeout - gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); + // Reset protocol timeout + gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); - // Attempt to detect GPS hw version - gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; - gpsState.autoConfigStep = 0; + // Attempt to detect GPS hw version + gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; + gpsState.autoConfigStep = 0; - do { - pollVersion(); - gpsState.autoConfigStep++; - ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + do { + pollVersion(); + gpsState.autoConfigStep++; + ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); + } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); gpsState.autoConfigStep = 0; ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; @@ -1062,7 +1062,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) { pollVersion(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - } + } pollGnssCapabilities(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 518cdc61aa1..51ca9d03a73 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1080,7 +1080,7 @@ static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *at if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { TEXT_ATTRIBUTES_ADD_BLINK(*attr); -} + } } void osdCrosshairPosition(uint8_t *x, uint8_t *y) @@ -1114,7 +1114,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes if (useScaled) { buff[0] = SYM_SCALE; } else { - buff[0] = SYM_BLANK; + buff[0] = SYM_BLANK; } buff[1] = SYM_THR; if (navigationIsControllingThrottle()) { @@ -1315,7 +1315,11 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen } } - if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { + if (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); float poiAngle = DEGREES_TO_RADIANS(directionToPoi); @@ -1429,7 +1433,11 @@ static void osdDisplayTelemetry(void) static uint16_t trk_bearing = 0; if (ARMING_FLAG(ARMED)) { - if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)){ + if (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ){ if (GPS_distanceToHome > 5) { trk_bearing = GPS_directionToHome; trk_bearing += 360 + 180; @@ -1729,7 +1737,7 @@ static bool osdDrawSingleElement(uint8_t item) // Shown in Ah buff[mah_digits] = SYM_AH; } else { - // Shown in mAh + // Shown in mAh buff[mah_digits] = SYM_MAH; } buff[mah_digits + 1] = '\0'; @@ -1786,12 +1794,16 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_SAT_L; buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); +#ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { strcpy(buff + 2, "ES"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { - strcpy(buff + 2, "X!"); + } else +#endif + if (!STATE(GPS_FIX)) { + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + strcpy(buff + 2, "X!"); + } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; @@ -1843,7 +1855,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); } @@ -2331,11 +2347,19 @@ static bool osdDrawSingleElement(uint8_t item) osdCrosshairPosition(&elemPosX, &elemPosY); osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); - if (osdConfig()->hud_homing && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if (osdConfig()->hud_homing && (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { osdHudDrawHoming(elemPosX, elemPosY); } - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && isImuHeadingValid()) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && isImuHeadingValid()) { if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { osdHudClear(); @@ -2959,8 +2983,12 @@ static bool osdDrawSingleElement(uint8_t item) digits = 4U; } #endif - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); @@ -3030,7 +3058,11 @@ static bool osdDrawSingleElement(uint8_t item) int32_t value = 0; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))&& gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); @@ -3182,7 +3214,11 @@ static bool osdDrawSingleElement(uint8_t item) STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); int digits = osdConfig()->plus_code_digits; int digitsRemoved = osdConfig()->plus_code_short * 2; - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); } else { // +codes with > 8 digits have a + at the 9th digit @@ -4112,13 +4148,13 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); } else if (page == 0) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); } else if (page == 1) { displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); } if (isSinglePageStatsCompatible || page == 0) { - if (feature(FEATURE_GPS)) { + if (feature(FEATURE_GPS)) { if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); @@ -4130,32 +4166,32 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } else { - displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); - osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); + osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); - osdGenerateAverageVelocityStr(buff); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); + osdGenerateAverageVelocityStr(buff); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); - osdFormatDistanceStr(buff, stats.max_distance*100); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); + osdFormatDistanceStr(buff, stats.max_distance*100); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); - osdFormatDistanceStr(buff, getTotalTravelDistance()); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); + osdFormatDistanceStr(buff, getTotalTravelDistance()); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); - osdFormatAltitudeStr(buff, stats.max_altitude); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); + osdFormatAltitudeStr(buff, stats.max_altitude); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - switch (rxConfig()->serialrx_provider) { - case SERIALRX_CRSF: + switch (rxConfig()->serialrx_provider) { + case SERIALRX_CRSF: if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :"); itoa(stats.min_rssi, buff, 10); @@ -4168,172 +4204,172 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - break; - default: - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } - - displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); - uint16_t flySeconds = getFlightTime(); - uint16_t flyMinutes = flySeconds / 60; - flySeconds %= 60; - uint16_t flyHours = flyMinutes / 60; - flyMinutes %= 60; - tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); - displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); - } - - if (isSinglePageStatsCompatible || page == 1) { - if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { - displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); - osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); - } - tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - if (feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_AMP); + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } + + displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); + uint16_t flySeconds = getFlightTime(); + uint16_t flyMinutes = flySeconds / 60; + flySeconds %= 60; + uint16_t flyHours = flyMinutes / 60; + flyMinutes %= 60; + tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); + displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); + } - displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); + if (isSinglePageStatsCompatible || page == 1) { + if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { + displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); } else { - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_WH); + displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); } + tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); displayWrite(osdDisplayPort, statValuesX, top++, buff); - int32_t totalDistance = getTotalTravelDistance(); - bool moreThanAh = false; - bool efficiencyValid = totalDistance >= 10000; - if (feature(FEATURE_GPS)) { - displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); - uint8_t digits = 3U; // Total number of digits (including decimal point) - #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Add one digit so no switch to scaled decimal occurs above 99 - digits = 4U; - } - #endif - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_MI_0; - buff[4] = SYM_MAH_MI_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; + if (feature(FEATURE_CURRENT_METER)) { + displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_AMP); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); + } else { + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_WH); + } + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + int32_t totalDistance = getTotalTravelDistance(); + bool moreThanAh = false; + bool efficiencyValid = totalDistance >= 10000; + if (feature(FEATURE_GPS)) { + displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); + uint8_t digits = 3U; // Total number of digits (including decimal point) + #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values + if (isBfCompatibleVideoSystem(osdConfig())) { + // Add one digit so no switch to scaled decimal occurs above 99 + digits = 4U; } - } - break; - case OSD_UNIT_GA: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + #endif + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_MI_0; + buff[4] = SYM_MAH_MI_1; + buff[5] = '\0'; + } } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_NM_0; - buff[4] = SYM_MAH_NM_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } } - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + break; + case OSD_UNIT_GA: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_NM_0; + buff[4] = SYM_MAH_NM_1; + buff[5] = '\0'; + } } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; - buff[5] = '\0'; + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } } - } - break; + break; + } + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); } - } - const float max_gforce = accGetMeasuredMaxG(); - displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); - osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + const float max_gforce = accGetMeasuredMaxG(); + displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); + const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); const float acc_extremes_min = acc_extremes[Z].min; const float acc_extremes_max = acc_extremes[Z].max; - displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); + displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff),"/"); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b8ddd058cb3..7b837e8ceef 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -786,7 +786,11 @@ static void osdDJIEfficiencyMahPerKM(char *buff) timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1, US2S(efficiencyTimeDelta)); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 47d644b70b5..f289a4ab27a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2378,6 +2378,7 @@ bool validateRTHSanityChecker(void) return true; } +#ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { //disable sanity checks in GPS estimation mode //when estimated GPS fix is replaced with real fix, coordinates may jump @@ -2386,6 +2387,7 @@ bool validateRTHSanityChecker(void) posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; return true; } +#endif // Check at 10Hz rate if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { @@ -3993,7 +3995,11 @@ void updateWpMissionPlanner(void) { static timeMs_t resetTimerStart = 0; if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) { - const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)); + const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ); posControl.flags.wpMissionPlannerActive = true; if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5b1abf0f88f..717f0df0e75 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -210,7 +210,9 @@ typedef struct positionEstimationConfig_s { uint8_t use_gps_no_baro; +#ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; +#endif } positionEstimationConfig_t; PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 51aa03b5d43..405a06b4a59 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -313,13 +313,13 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; - posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; + posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; + posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; - // turn direction to next waypoint - loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right + // turn direction to next waypoint + loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right - needToCalculateCircularLoiter = true; + needToCalculateCircularLoiter = true; } posControl.flags.wpTurnSmoothingActive = true; } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 9fed0f69786..50f961f1d6b 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -92,8 +92,9 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, - +#ifdef USE_GPS_FIX_ESTIMATION .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT +#endif ); #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } @@ -152,12 +153,14 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs) bool isGlitching = false; +#ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { //disable sanity checks in GPS estimation mode //when estimated GPS fix is replaced with real fix, coordinates may jump previousTime = 0; return true; } +#endif if (previousTime == 0) { isGlitching = false; @@ -210,8 +213,16 @@ void onNewGPSData(void) newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { - if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + if (!(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { isFirstGPSUpdate = true; return; } @@ -492,7 +503,11 @@ static bool navIsAccelerationUsable(void) static bool navIsHeadingUsable(void) { - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { // If we have GPS - we need true IMU north (valid heading) return isImuHeadingValid(); } @@ -507,7 +522,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) /* Figure out if we have valid position data from our data sources */ uint32_t newFlags = 0; - if ((sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) && posControl.gpsOrigin.valid && + if ((sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { @@ -704,7 +723,11 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) { - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && navIsHeadingUsable()) { + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) && navIsHeadingUsable()) { static timeUs_t lastUpdateTimeUs = 0; if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f4600b73511..19ef76e5155 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -471,8 +471,9 @@ static int logicConditionCompute( } else { return false; } - break; + break; +#ifdef USE_GPS_FIX_ESTIMATION case LOGIC_CONDITION_DISABLE_GPS_FIX: if (operandA > 0) { LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); @@ -480,8 +481,9 @@ static int logicConditionCompute( LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); } return true; - break; - + break; +#endif + default: return false; break; @@ -663,9 +665,12 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: +#ifdef USE_GPS_FIX_ESTIMATION if ( STATE(GPS_ESTIMATED_FIX) ){ return gpsSol.numSat; //99 - } else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + } else +#endif + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { return 0; } else { return gpsSol.numSat; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 69565393911..1dd197be9ff 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -81,7 +81,9 @@ typedef enum { LOGIC_CONDITION_TIMER = 49, LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_APPROX_EQUAL = 51, +#ifdef USE_GPS_FIX_ESTIMATION LOGIC_CONDITION_DISABLE_GPS_FIX = 52, +#endif LOGIC_CONDITION_LAST = 53, } logicOperation_e; @@ -186,7 +188,9 @@ typedef enum { LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10), +#ifdef USE_GPS_FIX_ESTIMATION LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11), +#endif } logicConditionsGlobalFlags_t; typedef enum { diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index cc874b6bb81..56c4d12417b 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -43,12 +43,10 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void) if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) { if (accIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } - } - else { + } else { if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; @@ -64,13 +62,13 @@ hardwareSensorStatus_e getHwCompassStatus(void) { #if defined(USE_MAG) #ifdef USE_SIMULATOR - if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) { + if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) { if (compassIsHealthy()) { return HW_SENSOR_OK; } else { return HW_SENSOR_UNHEALTHY; } - } + } #endif if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { @@ -96,7 +94,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void) { #if defined(USE_BARO) #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { + if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) { return HW_SENSOR_NONE; } else if (baroIsHealthy()) { @@ -104,7 +102,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void) } else { return HW_SENSOR_UNHEALTHY; } - } + } #endif if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) { if (baroIsHealthy()) { @@ -135,8 +133,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void) if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) { if (rangefinderIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -144,8 +141,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void) if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } @@ -161,8 +157,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void) if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { if (pitotIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -170,8 +165,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void) if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } @@ -187,8 +181,7 @@ hardwareSensorStatus_e getHwGPSStatus(void) if (sensors(SENSOR_GPS)) { if (isGPSHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -196,8 +189,7 @@ hardwareSensorStatus_e getHwGPSStatus(void) if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } @@ -213,8 +205,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void) if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { if (opflowIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -222,8 +213,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void) if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 2fae0a16922..e24898cdd14 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -215,14 +215,14 @@ STATIC_PROTOTHREAD(pitotThread) pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL); #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; } #endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; - } + } #endif ptYield(); @@ -248,14 +248,14 @@ STATIC_PROTOTHREAD(pitotThread) pitot.airSpeed = 0.0f; } #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitot.airSpeed = simulatorData.airSpeed; } #endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitot.airSpeed = fakePitotGetAirspeed(); - } + } #endif } diff --git a/src/main/target/common.h b/src/main/target/common.h index d18bdfb1a2e..242e03fc62c 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -56,6 +56,7 @@ #define USE_GPS_PROTO_MSP #define USE_TELEMETRY #define USE_TELEMETRY_LTM +#define USE_GPS_FIX_ESTIMATION // This is the shortest period in microseconds that the scheduler will allow #define SCHEDULER_DELAY_LIMIT 10 diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index a9630efa317..c468d2cfac5 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -147,7 +147,11 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) sbufWriteU16(dst, GPS_directionToHome); uint8_t gpsFlags = 0; - if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { + if (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { gpsFlags |= GPS_FLAGS_FIX; } if (STATE(GPS_FIX_HOME)) { diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index a9810eb8f6d..984b03caa6a 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -234,7 +234,11 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120); hottGPSMessage->climbrate3s = climbrate3s & 0xFF; - if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { + if (!(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX)) +#endif + ) { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } @@ -476,7 +480,11 @@ static bool processBinaryModeRequest(uint8_t address) switch (address) { #ifdef USE_GPS case 0x8A: - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { hottPrepareGPSResponse(&hottGPSMessage); hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage)); return true; diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 1bab900776a..3c7dfd2875a 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -136,7 +136,11 @@ static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8 static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { #if defined(USE_GPS) uint8_t fix = 0; - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { if (gpsSol.fixType == GPS_NO_FIX) fix = 1; else if (gpsSol.fixType == GPS_FIX_2D) fix = 2; else if (gpsSol.fixType == GPS_FIX_3D) fix = 3; @@ -196,7 +200,11 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0 uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()]; #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { status += gpsSol.numSat * 1000; status += fix * 100; if (STATE(GPS_FIX_HOME)) status += 500; @@ -206,72 +214,128 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { return sendIbusMeasurement2(address, status); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north) #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7 #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else #endif return sendIbusMeasurement2(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t #endif return sendIbusMeasurement4(address, 0); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m #if defined(USE_GPS) - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t #endif return sendIbusMeasurement2(address, 0); } diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 53de96cc5d4..b0ffad84347 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -575,7 +575,11 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); if (!allSensorsActive) { - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { enableGpsTelemetry(true); allSensorsActive = true; } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index e194cfed937..84dccddb869 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -117,7 +117,11 @@ void ltm_gframe(sbuf_t *dst) uint8_t gps_fix_type = 0; int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0; - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { if (gpsSol.fixType == GPS_NO_FIX) gps_fix_type = 1; else if (gpsSol.fixType == GPS_FIX_2D) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index c0759265808..3090ed68dc7 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -523,7 +523,11 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) { uint8_t gpsFixType = 0; - if (!(sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) return; if (gpsSol.fixType == GPS_NO_FIX) @@ -638,7 +642,11 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_GPS) // use ground speed if source available - if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { mavGroundSpeed = gpsSol.groundSpeed / 100.0f; } #endif diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index a0537bfcd5c..9b1f7cc6403 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -348,7 +348,11 @@ static void sendSMS(void) ZERO_FARRAY(pluscode_url); - if ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) { + if ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { groundSpeed = gpsSol.groundSpeed / 100; char buf[20]; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 64a6559f1d0..9d804be52d8 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -414,7 +414,11 @@ static bool smartPortShouldSendGPSData(void) // or the craft has never been armed yet. This way if GPS stops working // while in flight, the user will easily notice because the sensor will stop // updating. - return feature(FEATURE_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); + return feature(FEATURE_GPS) && (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + || !ARMING_FLAG(WAS_EVER_ARMED)); } void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index d8a8c15c33a..c341363f160 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -303,7 +303,11 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs) uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint8_t hdopBcd, gpsFlags; - if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) || gpsSol.numSat < 6) { + if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) || gpsSol.numSat < 6) { return false; } @@ -371,7 +375,11 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs) uint8_t numSatBcd, altitudeHighBcd; bool timeProvided = false; - if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))|| gpsSol.numSat < 6) { + if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )|| gpsSol.numSat < 6) { return false; } From 4602a837fbce0ce2226b9c6c2b3ca3db31705b69 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 9 Aug 2023 10:14:57 +0200 Subject: [PATCH 053/199] fixed unit test error --- src/main/telemetry/hott.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 984b03caa6a..8e2e7d8c075 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -234,11 +234,12 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120); hottGPSMessage->climbrate3s = climbrate3s & 0xFF; - if (!(STATE(GPS_FIX) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX)) +#ifdef USE_GPS_FIX_ESTIMATION + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) +#else + if (!(STATE(GPS_FIX))) #endif - ) { + { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } From 380ef16f799aca27b819aa9fc8f2a26ebac56d39 Mon Sep 17 00:00:00 2001 From: shota Date: Thu, 10 Aug 2023 01:07:48 +0900 Subject: [PATCH 054/199] trial --- src/main/drivers/pwm_mapping.c | 2 +- src/main/fc/runtime_config.h | 1 + src/main/fc/settings.yaml | 2 +- src/main/flight/mixer.c | 5 +++++ src/main/flight/mixer.h | 3 ++- src/main/flight/mixer_profile.c | 4 ++-- src/main/flight/pid.c | 12 +++++++++++- src/main/flight/servos.c | 2 +- src/main/telemetry/mavlink.c | 3 +++ 9 files changed, 27 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index a40faf965cc..c8121830307 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -250,7 +250,7 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU } // Determine if timer belongs to motor/servo - if (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER) { + if (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER || currentMixerConfig.platformType == PLATFORM_TAILSITTER) { // Multicopter // Make sure first motorCount outputs get assigned to motor diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 16d1b411df8..8764ea9f824 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -139,6 +139,7 @@ typedef enum { FW_HEADING_USE_YAW = (1 << 24), ANTI_WINDUP_DEACTIVATED = (1 << 25), LANDING_DETECTED = (1 << 26), + TAILSITTER = (1 << 27), //offset the pitch angle and swap roll/yaw controls } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 80540cc3968..3eb8ac78630 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -111,7 +111,7 @@ tables: values: ["PERCENT", "MAH", "MWH"] enum: smartportFuelUnit_e - name: platform_type - values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT"] + values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT", "TAILSITTER"] - name: output_mode values: ["AUTO", "MOTORS", "SERVOS"] enum: outputMode_e diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ab8a701bc74..a377b381bda 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -152,6 +152,7 @@ void mixerUpdateStateFlags(void) DISABLE_STATE(BOAT); DISABLE_STATE(AIRPLANE); DISABLE_STATE(MOVE_FORWARD_ONLY); + DISABLE_STATE(TAILSITTER); if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) { ENABLE_STATE(FIXED_WING_LEGACY); @@ -172,6 +173,10 @@ void mixerUpdateStateFlags(void) } else if (currentMixerConfig.platformType == PLATFORM_TRICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); + } else if (currentMixerConfig.platformType == PLATFORM_TAILSITTER) { + ENABLE_STATE(MULTIROTOR); + ENABLE_STATE(ALTITUDE_CONTROL); + ENABLE_STATE(TAILSITTER); } else if (currentMixerConfig.platformType == PLATFORM_HELICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 871332447ef..ee7f29686ce 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -39,7 +39,8 @@ typedef enum { PLATFORM_TRICOPTER = 3, PLATFORM_ROVER = 4, PLATFORM_BOAT = 5, - PLATFORM_OTHER = 6 + PLATFORM_OTHER = 6, + PLATFORM_TAILSITTER = 7, } flyingPlatformType_e; diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 044cb027abd..9afd5686355 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -200,8 +200,8 @@ bool checkMixerProfileHotSwitchAvalibility(void) #endif uint8_t platform_type0 = mixerConfigByIndex(0)->platformType; uint8_t platform_type1 = mixerConfigByIndex(1)->platformType; - bool platform_type_mc0 = (platform_type0 == PLATFORM_MULTIROTOR) || (platform_type0 == PLATFORM_TRICOPTER); - bool platform_type_mc1 = (platform_type1 == PLATFORM_MULTIROTOR) || (platform_type1 == PLATFORM_TRICOPTER); + bool platform_type_mc0 = (platform_type0 == PLATFORM_MULTIROTOR) || (platform_type0 == PLATFORM_TRICOPTER || (platform_type0 == PLATFORM_TAILSITTER)); + bool platform_type_mc1 = (platform_type1 == PLATFORM_MULTIROTOR) || (platform_type1 == PLATFORM_TRICOPTER || (platform_type1 == PLATFORM_TAILSITTER)); bool is_mcfw_switching = platform_type_mc0 ^ platform_type_mc1; if ((!MCFW_hotswap_available) && is_mcfw_switching) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f8728a3cefd..6d0931742fa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1085,8 +1085,18 @@ void FAST_CODE pidController(float dT) } for (int axis = 0; axis < 3; axis++) { + int gyro_axis = axis; + // float noinvert=1.0f; + // if (STATE(TAILSITTER)){ + // if (axis == FD_ROLL) { + // gyro_axis = FD_YAW; + // } else if (axis == FD_YAW) { + // gyro_axis = FD_ROLL; + // noinvert=-1.0f; + // } + // } // Step 1: Calculate gyro rates - pidState[axis].gyroRate = gyro.gyroADCf[axis]; + pidState[axis].gyroRate = gyro.gyroADCf[gyro_axis] * noinvert; // Step 2: Read target float rateTarget; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 4d4bb497d19..485f8d3684b 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -264,7 +264,7 @@ void servoMixer(float dT) // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) && - (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER)) { + (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER || currentMixerConfig.platformType == PLATFORM_TAILSITTER)) { input[INPUT_STABILIZED_YAW] *= -1; } } diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 30237c28552..a611e6def9d 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -684,6 +684,9 @@ void mavlinkSendHUDAndHeartbeat(void) case PLATFORM_TRICOPTER: mavSystemType = MAV_TYPE_TRICOPTER; break; + case PLATFORM_TAILSITTER: + mavSystemType = MAV_TYPE_QUADROTOR; + break; case PLATFORM_AIRPLANE: mavSystemType = MAV_TYPE_FIXED_WING; break; From dc1d2e8d4e79081c381871a97394007a51cfaef2 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 10 Aug 2023 00:34:12 +0200 Subject: [PATCH 055/199] ignore project file --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index ea13b9f1789..6f74144b06b 100644 --- a/.gitignore +++ b/.gitignore @@ -31,3 +31,4 @@ README.pdf # local changes only make/local.mk +/inav.code-workspace From 20bfb88094637d75c6690a5f3ee36ee6803614ba Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 11 Aug 2023 02:09:09 +0900 Subject: [PATCH 056/199] initial cut on tail sitter support --- src/main/fc/runtime_config.h | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/imu.c | 28 ++++++++++++++++++++++++++++ src/main/flight/mixer.h | 4 ++-- src/main/flight/pid.c | 13 +------------ src/main/sensors/boardalignment.c | 8 +++++++- 6 files changed, 40 insertions(+), 17 deletions(-) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 8764ea9f824..94eb23bd6a6 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -139,7 +139,7 @@ typedef enum { FW_HEADING_USE_YAW = (1 << 24), ANTI_WINDUP_DEACTIVATED = (1 << 25), LANDING_DETECTED = (1 << 26), - TAILSITTER = (1 << 27), //offset the pitch angle and swap roll/yaw controls + TAILSITTER = (1 << 27), //offset the pitch angle by 90 degrees } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3eb8ac78630..0f1ccc22922 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1142,7 +1142,7 @@ groups: field: mixer_config.motorDirectionInverted type: bool - name: platform_type - description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" + description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\", \"TAILSITTER\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" default_value: "MULTIROTOR" field: mixer_config.platformType type: uint8_t diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index c9d0f0aacbf..3ee0d303b6a 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -660,6 +660,33 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF lastspeed = currentspeed; } +fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){ + static bool firstRun = true; + static fpQuaternion_t qNormal2Tail; + static fpQuaternion_t qTail2Normal; + if(firstRun){ + fpAxisAngle_t axisAngle; + axisAngle.axis.x = 0; + axisAngle.axis.y = 1; + axisAngle.axis.z = 0; + axisAngle.angle = DEGREES_TO_RADIANS(90); + axisAngleToQuaternion(&qNormal2Tail, &axisAngle); + quaternionConjugate(&qTail2Normal, &qNormal2Tail); + firstRun = false; + } + return normal2tail ? &qNormal2Tail : &qTail2Normal; +} + +void imuUpdateTailSitter(void) +{ + static bool lastTailSitter=false; + if (((bool)STATE(TAILSITTER)) != lastTailSitter){ + fpQuaternion_t* rotation_for_tailsitter= getTailSitterQuaternion(STATE(TAILSITTER)); + quaternionMultiply(&orientation, &orientation, rotation_for_tailsitter); + } + lastTailSitter = STATE(TAILSITTER); +} + static void imuCalculateEstimatedAttitude(float dT) { #if defined(USE_MAG) @@ -766,6 +793,7 @@ static void imuCalculateEstimatedAttitude(float dT) useCOG, courseOverGround, accWeight, magWeight); + imuUpdateTailSitter(); imuUpdateEulerAngles(); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ee7f29686ce..10c1878b3a2 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -39,8 +39,8 @@ typedef enum { PLATFORM_TRICOPTER = 3, PLATFORM_ROVER = 4, PLATFORM_BOAT = 5, - PLATFORM_OTHER = 6, - PLATFORM_TAILSITTER = 7, + PLATFORM_TAILSITTER = 6, + PLATFORM_OTHER = 7, } flyingPlatformType_e; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6d0931742fa..0a3bcde4b82 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1085,18 +1085,7 @@ void FAST_CODE pidController(float dT) } for (int axis = 0; axis < 3; axis++) { - int gyro_axis = axis; - // float noinvert=1.0f; - // if (STATE(TAILSITTER)){ - // if (axis == FD_ROLL) { - // gyro_axis = FD_YAW; - // } else if (axis == FD_YAW) { - // gyro_axis = FD_ROLL; - // noinvert=-1.0f; - // } - // } - // Step 1: Calculate gyro rates - pidState[axis].gyroRate = gyro.gyroADCf[gyro_axis] * noinvert; + pidState[axis].gyroRate = gyro.gyroADCf[axis]; // Step 2: Read target float rateTarget; diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 86e41880f89..d23293dcce6 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -28,6 +28,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "fc/runtime_config.h" #include "drivers/sensor.h" @@ -87,7 +88,7 @@ void updateBoardAlignment(int16_t roll, int16_t pitch) void applyBoardAlignment(float *vec) { - if (standardBoardAlignment) { + if (standardBoardAlignment && (!STATE(TAILSITTER))) { return; } @@ -97,6 +98,11 @@ void applyBoardAlignment(float *vec) vec[X] = lrintf(fpVec.x); vec[Y] = lrintf(fpVec.y); vec[Z] = lrintf(fpVec.z); + + if (STATE(TAILSITTER)) { + vec[X] = lrintf(fpVec.z); + vec[Z] = -lrintf(fpVec.x); + } } void FAST_CODE applySensorAlignment(float * dest, float * src, uint8_t rotation) From 34f6690e903d48c10888d785f04eb54e7f19f84f Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 11 Aug 2023 13:21:10 +0900 Subject: [PATCH 057/199] tailsitter support --- src/main/flight/imu.c | 2 +- src/main/flight/pid.c | 5 +++++ src/main/sensors/boardalignment.c | 15 +++++++++------ 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3ee0d303b6a..939f9c4ee7e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -669,7 +669,7 @@ fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){ axisAngle.axis.x = 0; axisAngle.axis.y = 1; axisAngle.axis.z = 0; - axisAngle.angle = DEGREES_TO_RADIANS(90); + axisAngle.angle = DEGREES_TO_RADIANS(-90); axisAngleToQuaternion(&qNormal2Tail, &axisAngle); quaternionConjugate(&qTail2Normal, &qNormal2Tail); firstRun = false; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a3bcde4b82..db24091acb6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1119,6 +1119,11 @@ void FAST_CODE pidController(float dT) if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) { //If axis angle override, get the correct angle from Logic Conditions float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis)); + + //apply 45 deg offset for tailsitter when isMixerTransitionMixing is activated + if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH){ + angleTarget += DEGREES_TO_DECIDEGREES(45); + } //Apply the Level PID controller pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index d23293dcce6..ccad08163af 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -46,6 +46,7 @@ static bool standardBoardAlignment = true; // board orientation correction static fpMat3_t boardRotMatrix; +static fpMat3_t tailRotMatrix; // no template required since defaults are zero PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0); @@ -70,6 +71,11 @@ void initBoardAlignment(void) rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles); } + fp_angles_t rotationAngles; + rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(0); + rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(900); + rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(0); + rotationMatrixFromAngles(&tailRotMatrix, &rotationAngles); } void updateBoardAlignment(int16_t roll, int16_t pitch) @@ -94,15 +100,12 @@ void applyBoardAlignment(float *vec) fpVector3_t fpVec = { .v = { vec[X], vec[Y], vec[Z] } }; rotationMatrixRotateVector(&fpVec, &fpVec, &boardRotMatrix); - + if (STATE(TAILSITTER)) { + rotationMatrixRotateVector(&fpVec, &fpVec, &tailRotMatrix); + } vec[X] = lrintf(fpVec.x); vec[Y] = lrintf(fpVec.y); vec[Z] = lrintf(fpVec.z); - - if (STATE(TAILSITTER)) { - vec[X] = lrintf(fpVec.z); - vec[Z] = -lrintf(fpVec.x); - } } void FAST_CODE applySensorAlignment(float * dest, float * src, uint8_t rotation) From 084020335c8c4763c8f360982a9898bf0ef2bee0 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 12 Aug 2023 15:41:10 +0900 Subject: [PATCH 058/199] update tailsitter documents --- docs/MixerProfile.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index b30753e631a..432861009bf 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -204,6 +204,9 @@ save When `mixer_switch_on_rth`:`OFF` and `mixer_switch_on_land`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all. +### TailSitter support +TailSitter is supported by add a 90deg offset to the board alignment. Set the board aliment normally in the mixer_profile for FW mode(`set platform_type = AIRPLANE`), The motor trust axis should be same direction as the airplane nose. Then, in the mixer_profile for takeoff and landing `set platform_type = TAILSITTER`. The `TAILSITTER` platform type is same as `MULTIROTOR` platform type, expect for a 90 deg board alignment offset. In `TAILSITTER` mixer_profile, when motor trust/airplane nose is pointing to the sky, 'airplane bottom'/'multi rotor front' should facing forward in model preview. Set the motor/servo mixer according to multirotor orientation, Model should roll around geography's longitudinal axis, the roll axis of `TAILSITTER` will be yaw axis of `AIRPLANE`. In addition, When `MIXER TRANSITION` input is activated, a 45deg offset will be add to the target angle for angle mode. + ## Happy flying Remember that this is currently an emerging capability: From d186a8b51079d6215d178b7ada4a2d97b72c6902 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Sat, 19 Aug 2023 00:08:21 -0400 Subject: [PATCH 059/199] Create FLYWOOF405S_AIO --- src/main/target/FLYWOOF405S_AIO | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/main/target/FLYWOOF405S_AIO diff --git a/src/main/target/FLYWOOF405S_AIO b/src/main/target/FLYWOOF405S_AIO new file mode 100644 index 00000000000..8b137891791 --- /dev/null +++ b/src/main/target/FLYWOOF405S_AIO @@ -0,0 +1 @@ + From 56dc75b769d48a2053bf9c3ac369efea191d3faa Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Sat, 19 Aug 2023 00:10:43 -0400 Subject: [PATCH 060/199] Delete FLYWOOF405S_AIO --- src/main/target/FLYWOOF405S_AIO | 1 - 1 file changed, 1 deletion(-) delete mode 100644 src/main/target/FLYWOOF405S_AIO diff --git a/src/main/target/FLYWOOF405S_AIO b/src/main/target/FLYWOOF405S_AIO deleted file mode 100644 index 8b137891791..00000000000 --- a/src/main/target/FLYWOOF405S_AIO +++ /dev/null @@ -1 +0,0 @@ - From 2efa875baf8bb7988d140f64c9e7536cf084ee83 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Sat, 19 Aug 2023 00:14:55 -0400 Subject: [PATCH 061/199] Create CMakeLists --- src/main/target/FLYWOOF405S_AIO/CMakeLists | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/main/target/FLYWOOF405S_AIO/CMakeLists diff --git a/src/main/target/FLYWOOF405S_AIO/CMakeLists b/src/main/target/FLYWOOF405S_AIO/CMakeLists new file mode 100644 index 00000000000..f7a888b2424 --- /dev/null +++ b/src/main/target/FLYWOOF405S_AIO/CMakeLists @@ -0,0 +1 @@ +target_stm32f405xg(FLYWOOF405S_AIO) From ac14e690c294ca3ea7c1d1c5f848289bae1b30b6 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Sat, 19 Aug 2023 00:15:17 -0400 Subject: [PATCH 062/199] Add files via upload --- src/main/target/FLYWOOF405S_AIO/target.c | 38 +++++ src/main/target/FLYWOOF405S_AIO/target.h | 175 +++++++++++++++++++++++ 2 files changed, 213 insertions(+) create mode 100644 src/main/target/FLYWOOF405S_AIO/target.c create mode 100644 src/main/target/FLYWOOF405S_AIO/target.h diff --git a/src/main/target/FLYWOOF405S_AIO/target.c b/src/main/target/FLYWOOF405S_AIO/target.c new file mode 100644 index 00000000000..1191b9f1813 --- /dev/null +++ b/src/main/target/FLYWOOF405S_AIO/target.c @@ -0,0 +1,38 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,3,2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,0,2) + + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(1,7,5) + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYWOOF405S_AIO/target.h b/src/main/target/FLYWOOF405S_AIO/target.h new file mode 100644 index 00000000000..3ba0063d96f --- /dev/null +++ b/src/main/target/FLYWOOF405S_AIO/target.h @@ -0,0 +1,175 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "F4SA" +#define USBD_PRODUCT_STRING "FLYWOOF405S_AIO" + + +#define LED0 PC14 //Green +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_CS_PIN PB12 +#define BMI270_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_CS_PIN PB12 +#define ICM42605_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PB13 +#define USE_MPU_DATA_READY_SIGNAL + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + + + +// *************** SPI2 OSD *************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PB14// + +// *************** Onboard flash ******************** + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_BUS BUS_SPI3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + + +// *************** UART ***************************** +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_PIN PA8 +#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PA10 + +// 20230817 Added UART2 +//#define USE_UART2 +//#define UART2_TX_PIN PD5 +//#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN NONE +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART3 + + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** LED2812 ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA9 + +// *************** OTHERS ************************* +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) +#define CURRENT_METER_SCALE 250 + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 From 45b5c88767db33f5b0750b8acc271bb90b9a7451 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Sat, 19 Aug 2023 00:21:20 -0400 Subject: [PATCH 063/199] Update target.h --- src/main/target/FLYWOOF405S_AIO/target.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/target/FLYWOOF405S_AIO/target.h b/src/main/target/FLYWOOF405S_AIO/target.h index 3ba0063d96f..6cdc18d987e 100644 --- a/src/main/target/FLYWOOF405S_AIO/target.h +++ b/src/main/target/FLYWOOF405S_AIO/target.h @@ -115,11 +115,6 @@ #define UART1_TX_PIN PB6 #define UART1_RX_PIN PA10 -// 20230817 Added UART2 -//#define USE_UART2 -//#define UART2_TX_PIN PD5 -//#define UART2_RX_PIN PD6 - #define USE_UART3 #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 From 437a805366bf828aa0f580dc5bfe8fe4e0b773b1 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Sat, 19 Aug 2023 07:48:19 -0400 Subject: [PATCH 064/199] Update target.h Changed MPU6000 and BMI270 gyro orientation from CW180 to CW270 --- src/main/target/FLYWOOF405S_AIO/target.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/FLYWOOF405S_AIO/target.h b/src/main/target/FLYWOOF405S_AIO/target.h index 6cdc18d987e..eb6fe6c50d1 100644 --- a/src/main/target/FLYWOOF405S_AIO/target.h +++ b/src/main/target/FLYWOOF405S_AIO/target.h @@ -34,12 +34,12 @@ #define SPI1_MOSI_PIN PA7 #define USE_IMU_MPU6000 -#define IMU_MPU6000_ALIGN CW180_DEG +#define IMU_MPU6000_ALIGN CW270_DEG #define MPU6000_CS_PIN PB12 #define MPU6000_SPI_BUS BUS_SPI1 #define USE_IMU_BMI270 -#define IMU_BMI270_ALIGN CW180_DEG +#define IMU_BMI270_ALIGN CW270_DEG #define BMI270_CS_PIN PB12 #define BMI270_SPI_BUS BUS_SPI1 From a19e2ba006a765bf7169a5ac70e81ad2f0ff54d9 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Sat, 19 Aug 2023 23:28:18 -0400 Subject: [PATCH 065/199] Update target.h Added UART2 Bumped SERIAL_PORT_COUNT from 6 to 7 Changed TARGET_IO_PORTD mask from (BIT(2)) to 0xffff --- src/main/target/FLYWOOF405S_AIO/target.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/target/FLYWOOF405S_AIO/target.h b/src/main/target/FLYWOOF405S_AIO/target.h index eb6fe6c50d1..f52517a507d 100644 --- a/src/main/target/FLYWOOF405S_AIO/target.h +++ b/src/main/target/FLYWOOF405S_AIO/target.h @@ -115,6 +115,10 @@ #define UART1_TX_PIN PB6 #define UART1_RX_PIN PA10 +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + #define USE_UART3 #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 @@ -131,7 +135,7 @@ #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 -#define SERIAL_PORT_COUNT 6 +#define SERIAL_PORT_COUNT 7 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -165,6 +169,6 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTD 0xffff #define MAX_PWM_OUTPUT_PORTS 8 From 00bfca45fcbeca58881a3b5431f72286c945e2f8 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Sun, 20 Aug 2023 23:28:19 -0400 Subject: [PATCH 066/199] Update target.c Fixed motor map to match Cinerace20 O3 so they don't need to be remapped in INAV Configurator. --- src/main/target/FLYWOOF405S_AIO/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/FLYWOOF405S_AIO/target.c b/src/main/target/FLYWOOF405S_AIO/target.c index 1191b9f1813..be743d87be3 100644 --- a/src/main/target/FLYWOOF405S_AIO/target.c +++ b/src/main/target/FLYWOOF405S_AIO/target.c @@ -22,11 +22,11 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,3,2) + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,3,2) DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,0,2) DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(1,7,5) - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5) DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7) DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 From 3fe487d775e5441ca3115f0e394f10f6e1830f3a Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Mon, 21 Aug 2023 19:26:46 -0400 Subject: [PATCH 067/199] Update target.c Remapped Motors 1-4 to match motor direction (Props In or Props Out) arrows in INAV Configurator. --- src/main/target/FLYWOOF405S_AIO/target.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/target/FLYWOOF405S_AIO/target.c b/src/main/target/FLYWOOF405S_AIO/target.c index be743d87be3..30532746562 100644 --- a/src/main/target/FLYWOOF405S_AIO/target.c +++ b/src/main/target/FLYWOOF405S_AIO/target.c @@ -22,11 +22,10 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,3,2) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,0,2) - - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(1,7,5) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,0,2) + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 D(1,2,5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(1,3,2) + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 D(1,7,5) DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7) DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 From 46bba40ac19a65763c2e03a808396f8d1287360c Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Wed, 30 Aug 2023 02:37:53 -0400 Subject: [PATCH 068/199] Update target.h Changed TARGET_IO_PORTD mask from 0xffff to 0x0064 to strictly match PD5 and PD6 pins. --- src/main/target/FLYWOOF405S_AIO/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/FLYWOOF405S_AIO/target.h b/src/main/target/FLYWOOF405S_AIO/target.h index f52517a507d..c25dedfeb9b 100644 --- a/src/main/target/FLYWOOF405S_AIO/target.h +++ b/src/main/target/FLYWOOF405S_AIO/target.h @@ -169,6 +169,6 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTD 0x0064 #define MAX_PWM_OUTPUT_PORTS 8 From 46d8e3cb939763250d0f1a5223b617995d639e49 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Thu, 31 Aug 2023 16:32:45 -0700 Subject: [PATCH 069/199] Update target.c Anothe rearrangement of motor assignments to match S1, S2, S3, S4 in Inav Configurator and actually fly properly. --- src/main/target/FLYWOOF405S_AIO/target.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/FLYWOOF405S_AIO/target.c b/src/main/target/FLYWOOF405S_AIO/target.c index 30532746562..c7a49c74731 100644 --- a/src/main/target/FLYWOOF405S_AIO/target.c +++ b/src/main/target/FLYWOOF405S_AIO/target.c @@ -22,10 +22,10 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,0,2) - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 D(1,2,5) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(1,3,2) - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 D(1,7,5) + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7) DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 From 5f3aa7aa54dd6d83e0fae794c99db1b27f81312a Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Fri, 1 Sep 2023 06:13:44 -0700 Subject: [PATCH 070/199] Update CMakeLists Added SKIP_RELEASES tag --- src/main/target/FLYWOOF405S_AIO/CMakeLists | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/FLYWOOF405S_AIO/CMakeLists b/src/main/target/FLYWOOF405S_AIO/CMakeLists index f7a888b2424..6e3af8a3cbb 100644 --- a/src/main/target/FLYWOOF405S_AIO/CMakeLists +++ b/src/main/target/FLYWOOF405S_AIO/CMakeLists @@ -1 +1 @@ -target_stm32f405xg(FLYWOOF405S_AIO) +target_stm32f405xg(FLYWOOF405S_AIO SKIP_RELEASES) From 86b875da5a8ca58545c8f9e8dda556cc73f558cd Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 3 Sep 2023 22:31:20 +0200 Subject: [PATCH 071/199] allow gps fix estimation without compass --- src/main/io/gps.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 0e5d166c7c7..129d05c4173 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -226,14 +226,13 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) #ifdef USE_GPS_FIX_ESTIMATION bool canEstimateGPSFix(void) { -#if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) +#if defined(USE_GPS) && && defined(USE_BARO) //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_BARO) && baroIsHealthy() && - sensors(SENSOR_MAG) && compassIsHealthy() && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); #else From 6168348ca70425a0e375ed2af84f4a260c594f94 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:02:33 +0300 Subject: [PATCH 072/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 4ed5365e301..672f4845c2b 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -12,7 +12,7 @@ Plane should have the following sensors: - acceleromenter, gyroscope - barometer - GPS -- magnethometer +- magnethometer (optional, highly recommended) - pitot (optional) By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above unreachable spaces, plane will be lost. @@ -41,6 +41,23 @@ It is assumed, that plane will fly in roughly estimated direction to home positi *Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. +# Navigation without magnethometer + +Without magnethometer, navigation accuracy is very poor. The problem is heading drift. + +The longer plane flies without magnethometer or GPS, the bigger is course estimation error. + +After few minutes and few turns, "North" direction estimation can be completely broken. +In general, accuracy is enought to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with accasional GPS outages. + +![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da) + +(purple line - estimated position, black line - real position). + +It is recommened to use GPS fix estimation without magnethometer as last resort only. For example, if plane is flying above lake, landing means loss of plane. With GPS Fix estimation, plane will try to do RTH in very rought direction, instead of landing. + +It is up to user to estimate the risk of fly-away. + # Settings @@ -75,7 +92,7 @@ Example: 100 km/h = 100 * 27.77 = 2777 cm/s ![](Screenshots/programming_disable_gps_sensor_fix.png) -For testing purpoces, it is possible to disable GPS sensor fix from RC controller in programming tab: +For testing purposes, it is possible to disable GPS sensor fix from RC controller in programming tab: *GPS can be disabled only after: 1) initial GPS fix is acquired 2) in ARMED mode.* From 713b52176e2b0635adfe774fb5005f83280bfdb4 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:04:54 +0300 Subject: [PATCH 073/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 672f4845c2b..37cfa73df20 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -41,7 +41,7 @@ It is assumed, that plane will fly in roughly estimated direction to home positi *Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. -# Navigation without magnethometer +# Estimation without magnethometer Without magnethometer, navigation accuracy is very poor. The problem is heading drift. From 0829ef6f01f39d061de44311b87b4067cb419fdf Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:05:44 +0300 Subject: [PATCH 074/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 37cfa73df20..dcd1b2ac7ce 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -48,7 +48,7 @@ Without magnethometer, navigation accuracy is very poor. The problem is heading The longer plane flies without magnethometer or GPS, the bigger is course estimation error. After few minutes and few turns, "North" direction estimation can be completely broken. -In general, accuracy is enought to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with accasional GPS outages. +In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with accasional GPS outages. ![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da) From 3ee200de83568ca69d7f7f7efb50f6eb002f60f8 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:06:49 +0300 Subject: [PATCH 075/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index dcd1b2ac7ce..44487a6c062 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -48,7 +48,7 @@ Without magnethometer, navigation accuracy is very poor. The problem is heading The longer plane flies without magnethometer or GPS, the bigger is course estimation error. After few minutes and few turns, "North" direction estimation can be completely broken. -In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with accasional GPS outages. +In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with occasional GPS outages. ![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da) From f05dcc17e9e46d34134805afc123f20e26525734 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 4 Sep 2023 00:08:26 +0300 Subject: [PATCH 076/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 44487a6c062..ed67682353f 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -102,7 +102,7 @@ For testing purposes, it is possible to disable GPS sensor fix from RC controlle Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation. RTH is trigerred regradless of failsafe procedure selected in configurator. -# Expected error +# Expected error (mag + baro) Realistic expected error is up to 200m per 1km of flight path. In tests, 500m drift per 5km path was seen. From b7745af0f042ebff2fa2bb2ebbef45ee7505696a Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 3 Sep 2023 23:51:42 +0200 Subject: [PATCH 077/199] fixed compilation error --- src/main/io/gps.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index aa5450fc9b4..28b666c1ac9 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -231,7 +231,7 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) #ifdef USE_GPS_FIX_ESTIMATION bool canEstimateGPSFix(void) { -#if defined(USE_GPS) && && defined(USE_BARO) +#if defined(USE_GPS) && defined(USE_BARO) //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once From 5fd4e8e5ae92db72cc921daf1db08623a74fabe8 Mon Sep 17 00:00:00 2001 From: JulioCesarMatias Date: Tue, 12 Sep 2023 17:41:16 -0300 Subject: [PATCH 078/199] Add project to Bitbucket example --- src/main/sensors/compass.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 23d45ed7879..9c7ba99d9f9 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -485,4 +485,5 @@ void compassUpdate(timeUs_t currentTimeUs) magUpdatedAtLeastOnce = true; } + #endif From 2d8908a798cda8dc34e8cd122e0c14f222b7104c Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Wed, 27 Sep 2023 19:44:55 -0400 Subject: [PATCH 079/199] Changed TIM_USE_MC|FW_MOTOR to TIM_USE_OUTPUT_AUTO --- src/main/target/FLYWOOF405S_AIO/target.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/target/FLYWOOF405S_AIO/target.c b/src/main/target/FLYWOOF405S_AIO/target.c index c7a49c74731..0d51ec0c109 100644 --- a/src/main/target/FLYWOOF405S_AIO/target.c +++ b/src/main/target/FLYWOOF405S_AIO/target.c @@ -22,13 +22,13 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM From 378664c322f5eb225d96127b3e67111adc5dd5fe Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 29 Sep 2023 00:09:07 +0900 Subject: [PATCH 080/199] add compass changing aliment --- src/main/sensors/boardalignment.c | 12 +++++++++--- src/main/sensors/boardalignment.h | 4 +++- src/main/sensors/compass.c | 2 +- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index ccad08163af..360ddec4a16 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -92,6 +92,14 @@ void updateBoardAlignment(int16_t roll, int16_t pitch) initBoardAlignment(); } +void applyTailSitterAlignment(fpVector3_t *fpVec) +{ + if (!STATE(TAILSITTER)) { + return; + } + rotationMatrixRotateVector(fpVec, fpVec, &tailRotMatrix); +} + void applyBoardAlignment(float *vec) { if (standardBoardAlignment && (!STATE(TAILSITTER))) { @@ -100,9 +108,7 @@ void applyBoardAlignment(float *vec) fpVector3_t fpVec = { .v = { vec[X], vec[Y], vec[Z] } }; rotationMatrixRotateVector(&fpVec, &fpVec, &boardRotMatrix); - if (STATE(TAILSITTER)) { - rotationMatrixRotateVector(&fpVec, &fpVec, &tailRotMatrix); - } + applyTailSitterAlignment(&fpVec); vec[X] = lrintf(fpVec.x); vec[Y] = lrintf(fpVec.y); vec[Z] = lrintf(fpVec.z); diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index 6bf01272650..17cd08e5ff3 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -18,6 +18,7 @@ #pragma once #include "config/parameter_group.h" +#include "common/vector.h" typedef struct boardAlignment_s { int16_t rollDeciDegrees; @@ -30,4 +31,5 @@ PG_DECLARE(boardAlignment_t, boardAlignment); void initBoardAlignment(void); void updateBoardAlignment(int16_t roll, int16_t pitch); void applySensorAlignment(float * dest, float * src, uint8_t rotation); -void applyBoardAlignment(float *vec); \ No newline at end of file +void applyBoardAlignment(float *vec); +void applyTailSitterAlignment(fpVector3_t *vec); \ No newline at end of file diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 23d45ed7879..734a1e98224 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -472,7 +472,7 @@ void compassUpdate(timeUs_t currentTimeUs) fpVector3_t rotated; rotationMatrixRotateVector(&rotated, &v, &mag.dev.magAlign.externalRotation); - + applyTailSitterAlignment(&rotated); mag.magADC[X] = rotated.x; mag.magADC[Y] = rotated.y; mag.magADC[Z] = rotated.z; From 054603228db6a3c69cdd85fc1f262b92bbcdd5dd Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 5 Oct 2023 23:40:15 +0200 Subject: [PATCH 081/199] fixed: OSD CMS menu does not work in SITL --- src/main/drivers/serial_tcp.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index 1e2c78f1974..48522508ce8 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -265,13 +265,8 @@ uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance) uint32_t tcpTotalTxBytesFree(const serialPort_t *instance) { - tcpPort_t *port = (tcpPort_t*)instance; - - if (port->isClientConnected) { - return TCP_MAX_PACKET_SIZE; - } else { - return 0; - } + UNUSED(instance); + return TCP_MAX_PACKET_SIZE; } bool isTcpTransmitBufferEmpty(const serialPort_t *instance) From 1ea7aa7e48b5c9cc059855f44755b1978801b670 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 8 Oct 2023 00:59:46 +0900 Subject: [PATCH 082/199] moved tailsitter platform type to tailsitter_board_orientation --- src/main/fc/settings.yaml | 9 +++++++-- src/main/flight/mixer.c | 10 ++++++---- src/main/flight/mixer.h | 3 +-- src/main/flight/mixer_profile.c | 1 + src/main/flight/mixer_profile.h | 1 + src/main/flight/servos.c | 2 +- src/main/telemetry/mavlink.c | 3 --- 7 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bee6149147b..d3723d05af7 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -111,7 +111,7 @@ tables: values: ["PERCENT", "MAH", "MWH"] enum: smartportFuelUnit_e - name: platform_type - values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT", "TAILSITTER"] + values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT"] - name: tz_automatic_dst values: ["OFF", "EU", "USA"] enum: tz_automatic_dst_e @@ -1145,7 +1145,7 @@ groups: field: mixer_config.motorDirectionInverted type: bool - name: platform_type - description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\", \"TAILSITTER\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" + description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" default_value: "MULTIROTOR" field: mixer_config.platformType type: uint8_t @@ -1182,6 +1182,11 @@ groups: field: mixer_config.switchTransitionTimer min: 0 max: 200 + - name: tailsitter_board_orientation + description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + default_value: OFF + field: mixer_config.tailsitterBoardOrientation + type: bool diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d0d93a3348a..874f48c5c70 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -182,15 +182,17 @@ void mixerUpdateStateFlags(void) } else if (currentMixerConfig.platformType == PLATFORM_TRICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); - } else if (currentMixerConfig.platformType == PLATFORM_TAILSITTER) { - ENABLE_STATE(MULTIROTOR); - ENABLE_STATE(ALTITUDE_CONTROL); - ENABLE_STATE(TAILSITTER); } else if (currentMixerConfig.platformType == PLATFORM_HELICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); } + if (currentMixerConfig.tailsitterBoardOrientation) { + ENABLE_STATE(TAILSITTER); + } else { + DISABLE_STATE(TAILSITTER); + } + if (currentMixerConfig.hasFlaps) { ENABLE_STATE(FLAPERON_AVAILABLE); } else { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ba0a856be6e..9ee6a20654e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -41,8 +41,7 @@ typedef enum { PLATFORM_TRICOPTER = 3, PLATFORM_ROVER = 4, PLATFORM_BOAT = 5, - PLATFORM_TAILSITTER = 6, - PLATFORM_OTHER = 7, + PLATFORM_OTHER = 6 } flyingPlatformType_e; diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index de1cdecb3bd..de347feed3e 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -52,6 +52,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT, .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, + .tailsitterBoardOrientation = SETTING_TAILSITTER_BOARD_ORIENTATION_DEFAULT, }); for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index bf52d45c3d9..073e256eb1e 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,6 +18,7 @@ typedef struct mixerConfig_s { bool PIDProfileLinking; bool automated_switch; int16_t switchTransitionTimer; + bool tailsitterBoardOrientation; } mixerConfig_t; typedef struct mixerProfile_s { mixerConfig_t mixer_config; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 485f8d3684b..4d4bb497d19 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -264,7 +264,7 @@ void servoMixer(float dT) // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) && - (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER || currentMixerConfig.platformType == PLATFORM_TAILSITTER)) { + (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER)) { input[INPUT_STABILIZED_YAW] *= -1; } } diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index a611e6def9d..30237c28552 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -684,9 +684,6 @@ void mavlinkSendHUDAndHeartbeat(void) case PLATFORM_TRICOPTER: mavSystemType = MAV_TYPE_TRICOPTER; break; - case PLATFORM_TAILSITTER: - mavSystemType = MAV_TYPE_QUADROTOR; - break; case PLATFORM_AIRPLANE: mavSystemType = MAV_TYPE_FIXED_WING; break; From 4fd677245844eef0bd3bb2153dce8ed3f3af81a1 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 8 Oct 2023 01:35:46 +0900 Subject: [PATCH 083/199] bug fixes --- docs/Settings.md | 12 +++++++++++- src/main/sensors/boardalignment.c | 29 ++++++++++++----------------- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index bb26b36f5fd..74cd5d52b8c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4894,7 +4894,7 @@ _// TODO_ ### platform_type -Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT", "TAILSITTER". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented +Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | Default | Min | Max | | --- | --- | --- | @@ -5572,6 +5572,16 @@ Delay before disarming when requested by switch (ms) [0-1000] --- +### tailsitter_board_orientation + +If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### telemetry_halfduplex S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 360ddec4a16..1f149faeff6 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -58,24 +58,19 @@ static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment) void initBoardAlignment(void) { - if (isBoardAlignmentStandard(boardAlignment())) { - standardBoardAlignment = true; - } else { - fp_angles_t rotationAngles; - - standardBoardAlignment = false; - - rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(boardAlignment()->rollDeciDegrees ); - rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(boardAlignment()->pitchDeciDegrees); - rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees ); - - rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles); - } + standardBoardAlignment=isBoardAlignmentStandard(boardAlignment()); fp_angles_t rotationAngles; - rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(0); - rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(900); - rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(0); - rotationMatrixFromAngles(&tailRotMatrix, &rotationAngles); + + rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(boardAlignment()->rollDeciDegrees ); + rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(boardAlignment()->pitchDeciDegrees); + rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees ); + + rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles); + fp_angles_t tailSitter_rotationAngles; + tailSitter_rotationAngles.angles.roll = DECIDEGREES_TO_RADIANS(0); + tailSitter_rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(900); + tailSitter_rotationAngles.angles.yaw = DECIDEGREES_TO_RADIANS(0); + rotationMatrixFromAngles(&tailRotMatrix, &tailSitter_rotationAngles); } void updateBoardAlignment(int16_t roll, int16_t pitch) From b5e3cb28d4a606ac18a9f7d2ab2e74e4ee0c9699 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 8 Oct 2023 01:38:16 +0900 Subject: [PATCH 084/199] update docs --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 74cd5d52b8c..67e4b554652 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5574,7 +5574,7 @@ Delay before disarming when requested by switch (ms) [0-1000] ### tailsitter_board_orientation -If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. +Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d3723d05af7..10d601d8712 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1183,7 +1183,7 @@ groups: min: 0 max: 200 - name: tailsitter_board_orientation - description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode" default_value: OFF field: mixer_config.tailsitterBoardOrientation type: bool From cfb63dd56a424a27fc5e5939228036aea0387e57 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 16 Oct 2023 10:52:02 +0900 Subject: [PATCH 085/199] parameter rename --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 4 ++-- src/main/flight/mixer.c | 2 +- src/main/flight/mixer_profile.c | 2 +- src/main/flight/mixer_profile.h | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 67e4b554652..968f2e49f5a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5572,7 +5572,7 @@ Delay before disarming when requested by switch (ms) [0-1000] --- -### tailsitter_board_orientation +### tailsitter_orientation_offset Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 10d601d8712..bc5429b3384 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1182,10 +1182,10 @@ groups: field: mixer_config.switchTransitionTimer min: 0 max: 200 - - name: tailsitter_board_orientation + - name: tailsitter_orientation_offset description: "Apply a 90 deg pitch offset in sensor aliment for tailsitter flying mode" default_value: OFF - field: mixer_config.tailsitterBoardOrientation + field: mixer_config.tailsitterOrientationOffset type: bool diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 874f48c5c70..834841e6580 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -187,7 +187,7 @@ void mixerUpdateStateFlags(void) ENABLE_STATE(ALTITUDE_CONTROL); } - if (currentMixerConfig.tailsitterBoardOrientation) { + if (currentMixerConfig.tailsitterOrientationOffset) { ENABLE_STATE(TAILSITTER); } else { DISABLE_STATE(TAILSITTER); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index de347feed3e..421dff17639 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -52,7 +52,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT, .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, - .tailsitterBoardOrientation = SETTING_TAILSITTER_BOARD_ORIENTATION_DEFAULT, + .tailsitterOrientationOffset = SETTING_TAILSITTER_ORIENTATION_OFFSET_DEFAULT, }); for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 073e256eb1e..a709309685f 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -18,7 +18,7 @@ typedef struct mixerConfig_s { bool PIDProfileLinking; bool automated_switch; int16_t switchTransitionTimer; - bool tailsitterBoardOrientation; + bool tailsitterOrientationOffset; } mixerConfig_t; typedef struct mixerProfile_s { mixerConfig_t mixer_config; From 0349bd70fe5164e1cb6299b3b2005a73f40ef55a Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 20 Oct 2023 09:15:03 +0900 Subject: [PATCH 086/199] logic condition RCcommand overide also works for angle mode --- src/main/flight/pid.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5e7f1edb132..fdea10dc435 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -584,7 +584,11 @@ int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis) static float computePidLevelTarget(flight_dynamics_index_t axis) { // This is ROLL/PITCH, run ANGLE/HORIZON controllers +#ifdef USE_PROGRAMMING_FRAMEWORK + float angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), pidProfile()->max_angle_inclination[axis]); +#else float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]); +#endif // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { From e17ae02b347799db85c1429d1ad098fe243c99b7 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 22 Oct 2023 14:21:46 +0900 Subject: [PATCH 087/199] ahrs mag gps fusion --- src/main/flight/imu.c | 65 ++++++++++++++++++++++++++++++++----------- 1 file changed, 48 insertions(+), 17 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index ed5cbcbfaa4..a7b61e4819a 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -77,6 +77,7 @@ #define SPIN_RATE_LIMIT 20 #define MAX_ACC_NEARNESS 0.2 // 20% or G error soft-accepted (0.8-1.2G) +#define MAX_MAG_NEARNESS 0.2 // 20% or magnetic field error soft-accepted (0.8-1.2) #define IMU_ROTATION_LPF 3 // Hz FASTRAM fpVector3_t imuMeasuredAccelBF; FASTRAM fpVector3_t imuMeasuredRotationBF; @@ -111,6 +112,8 @@ FASTRAM bool gpsHeadingInitialized; FASTRAM bool imuUpdated = false; +static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF); + PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2); PG_RESET_TEMPLATE(imuConfig_t, imuConfig, @@ -326,6 +329,15 @@ bool isGPSTrustworthy(void) return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6; } +static float imuCalculateMcMagCogWeight(void) +{ + float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF); + float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered)); + const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope)) * 4.0f; + wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f); + return wCoG; +} + static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; @@ -339,11 +351,13 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe /* Step 1: Yaw correction */ // Use measured magnetic field vector if (magBF || useCOG) { - static const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } }; - - fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } }; + float wMag; + float wCoG; + fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } }; + fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } }; if (magBF && vectorNormSquared(magBF) > 0.01f) { + wMag = bellCurve((vectorNormSquared(magBF) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); //TODO check if 1024 is correct fpVector3_t vMag; // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). @@ -369,13 +383,26 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) - vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth); + vectorCrossProduct(&vMagErr, &vMag, &vCorrectedMagNorth); // Rotate error back into body frame - quaternionRotateVector(&vErr, &vErr, &orientation); + quaternionRotateVector(&vMagErr, &vMagErr, &orientation); } } - else if (useCOG) { + if (useCOG) { + fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } }; + //vForward as trust vector + if STATE(MULTIROTOR){ + vForward.z = 1.0f; + }else{ + vForward.x = 1.0f; + } + + wCoG = scaleRangef(constrainf(gpsSol.groundSpeed, 300, 1200), 300, 1200, 0.0f, 1.0f); + if (STATE(MULTIROTOR)){ + //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading + wCoG *= imuCalculateMcMagCogWeight(); + } fpVector3_t vHeadingEF; // Use raw heading error (from GPS or whatever else) @@ -409,13 +436,16 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vectorNormalize(&vHeadingEF, &vHeadingEF); // error is cross product between reference heading and estimated heading (calculated in EF) - vectorCrossProduct(&vErr, &vCoG, &vHeadingEF); + vectorCrossProduct(&vCoGErr, &vCoG, &vHeadingEF); // Rotate error back into body frame - quaternionRotateVector(&vErr, &vErr, &orientation); + quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation); } } - + fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } }; + vectorScale(&vMagErr, &vMagErr, wMag); + vectorScale(&vCoGErr, &vCoGErr, wCoG); + vectorAdd(&vErr, &vMagErr, &vCoGErr); // Compute and apply integral feedback if enabled if (imuRuntimeConfig.dcm_ki_mag > 0.0f) { // Stop integrating if spinning beyond the certain limit @@ -535,10 +565,10 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) } } -static float imuCalculateAccelerometerWeightNearness(void) +static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF) { fpVector3_t accBFNorm; - vectorScale(&accBFNorm, &compansatedGravityBF, 1.0f / GRAVITY_CMSS); + vectorScale(&accBFNorm, accBF, 1.0f / GRAVITY_CMSS); const float accMagnitudeSq = vectorNormSquared(&accBFNorm); const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS); return accWeight_Nearness; @@ -672,20 +702,21 @@ static void imuCalculateEstimatedAttitude(float dT) bool useMag = false; bool useCOG = false; #if defined(USE_GPS) - if (STATE(FIXED_WING_LEGACY)) { + if (STATE(FIXED_WING_LEGACY) || STATE(MULTIROTOR)) { bool canUseCOG = isGPSHeadingValid(); - // Prefer compass (if available) + // Use compass (if available) if (canUseMAG) { useMag = true; gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails } - else if (canUseCOG) { + // Use GPS (if available) + if (canUseCOG) { if (gpsHeadingInitialized) { courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); useCOG = true; } - else { + else if (!canUseMAG) { // Re-initialize quaternion from known Roll, Pitch and GPS heading imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse); gpsHeadingInitialized = true; @@ -698,7 +729,7 @@ static void imuCalculateEstimatedAttitude(float dT) } } else { - // Multicopters don't use GPS heading + // other platform type don't use GPS heading if (canUseMAG) { useMag = true; } @@ -751,7 +782,7 @@ static void imuCalculateEstimatedAttitude(float dT) } compansatedGravityBF = imuMeasuredAccelBF #endif - float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(); + float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF); accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler); const bool useAcc = (accWeight > 0.001f); From 69ac08048e83b3e15640e417b312d9288d8767d9 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 22 Oct 2023 14:29:07 +0900 Subject: [PATCH 088/199] minor fix --- src/main/flight/imu.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a7b61e4819a..e93aac430bb 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -351,8 +351,8 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe /* Step 1: Yaw correction */ // Use measured magnetic field vector if (magBF || useCOG) { - float wMag; - float wCoG; + float wMag = 0.0f; + float wCoG = 0.0f; fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } }; fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } }; From 8e3ee38e518b51a70714e77fa6e8d1aa8e2cd269 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 22 Oct 2023 16:51:09 +0900 Subject: [PATCH 089/199] development of posestimator fusion --- src/main/fc/settings.yaml | 4 ++-- src/main/navigation/navigation_pos_estimator.c | 16 +++++++++------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 87daf753cf2..c719c4003f0 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2314,13 +2314,13 @@ groups: max: 100 default_value: 2.0 - name: inav_w_z_baro_p - description: "Weight of barometer measurements in estimated altitude and climb rate" + description: "Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors." field: w_z_baro_p min: 0 max: 10 default_value: 0.35 - name: inav_w_z_gps_p - description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes" + description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors." field: w_z_gps_p min: 0 max: 10 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 5c314bfddda..d83836bfb14 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -570,7 +570,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count - if (ctx->newFlags & EST_BARO_VALID) { + bool correctOK = false; + //use both baro and gps + + if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro)) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -589,7 +592,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } } - // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it + // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) && (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) || ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); @@ -614,10 +617,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); } - return true; + correctOK = true; } - else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) { - // If baro is not available - use GPS Z for correction on a plane + if (ctx->newFlags & EST_GPS_Z_VALID) { // Reset current estimate to GPS altitude if estimate not valid if (!(ctx->newFlags & EST_Z_VALID)) { ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z; @@ -637,10 +639,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); } - return true; + correctOK = true; } - return false; + return correctOK; } static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) From 00b46bc0d6e5828e55b4e6df6592beec5b267e5d Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 22 Oct 2023 18:16:19 +0900 Subject: [PATCH 090/199] development of posestimator fusion --- src/main/fc/settings.yaml | 4 ++-- src/main/navigation/navigation_pos_estimator.c | 16 +++++----------- 2 files changed, 7 insertions(+), 13 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c719c4003f0..254ad509ec5 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2324,13 +2324,13 @@ groups: field: w_z_gps_p min: 0 max: 10 - default_value: 0.2 + default_value: 0.5 - name: inav_w_z_gps_v description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors." field: w_z_gps_v min: 0 max: 10 - default_value: 0.1 + default_value: 0.5 - name: inav_w_xy_gps_p description: "Weight of GPS coordinates in estimated UAV position and speed." default_value: 1.0 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d83836bfb14..fa0aa5db0c2 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -571,9 +571,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; + + const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro is deference is too big //use both baro and gps - - if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro)) { + if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv)) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -602,14 +603,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; - // If GPS is available - also use GPS climb rate - if (ctx->newFlags & EST_GPS_Z_VALID) { - // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution - const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z; - const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f); - ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt; - } - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); // Accelerometer bias @@ -629,10 +622,11 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) else { // Altitude const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z; + const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z; ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt; ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; - ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt; + ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt; ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p); // Accelerometer bias From a4c959943d3b59f3db138e1527674ff3e3ae537c Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 23 Oct 2023 02:12:30 +0900 Subject: [PATCH 091/199] development of posestimator fusion --- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_pos_estimator.c | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 254ad509ec5..9d31a1e231d 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2330,7 +2330,7 @@ groups: field: w_z_gps_v min: 0 max: 10 - default_value: 0.5 + default_value: 1.0 - name: inav_w_xy_gps_p description: "Weight of GPS coordinates in estimated UAV position and speed." default_value: 1.0 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index fa0aa5db0c2..f8473a354f9 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -572,9 +572,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) bool correctOK = false; - const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro is deference is too big + const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro if difference is too big, baro is probably wrong //use both baro and gps - if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv)) { + if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv * 2)) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -584,9 +584,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } else { if (posEstimator.est.vel.z > 15) { - if (currentTimeUs > posEstimator.state.baroGroundTimeout) { - posEstimator.state.isBaroGroundValid = false; - } + posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true; } else { posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec From 04a9d8cbb8fa230e50dde13837b1be10baadf7e5 Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 24 Oct 2023 10:38:01 +0900 Subject: [PATCH 092/199] modify the weights --- src/main/flight/imu.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e93aac430bb..08373b6e5d0 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -351,13 +351,13 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe /* Step 1: Yaw correction */ // Use measured magnetic field vector if (magBF || useCOG) { - float wMag = 0.0f; - float wCoG = 0.0f; + float wMag = 1.0f; + float wCoG = 1.0f; fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } }; fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } }; if (magBF && vectorNormSquared(magBF) > 0.01f) { - wMag = bellCurve((vectorNormSquared(magBF) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); //TODO check if 1024 is correct + wMag *= bellCurve((vectorNormSquared(magBF) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); //TODO check if 1024 is correct fpVector3_t vMag; // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). @@ -397,11 +397,11 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe }else{ vForward.x = 1.0f; } - - wCoG = scaleRangef(constrainf(gpsSol.groundSpeed, 300, 1200), 300, 1200, 0.0f, 1.0f); if (STATE(MULTIROTOR)){ //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcMagCogWeight(); + //scale accroading to multirotor`s tilt angle + wCoG *= scaleRangef(constrainf(vForward.z, 0.98f, 0.90f), 0.98f, 0.90f, 0.0f, 1.0f); //cos(10deg) and cos(25deg) } fpVector3_t vHeadingEF; @@ -413,6 +413,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // (Rxx; Ryx) - measured (estimated) heading vector (EF) // (-cos(COG), sin(COG)) - reference heading vector (EF) + float airSpeed = gpsSol.groundSpeed; // Compute heading vector in EF from scalar CoG,x axis of accelerometer is pointing backwards. fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } }; #if defined(USE_WIND_ESTIMATOR) @@ -422,10 +423,11 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vectorScale(&vCoG, &vCoG, gpsSol.groundSpeed); vCoG.x += getEstimatedWindSpeed(X); vCoG.y -= getEstimatedWindSpeed(Y); + airSpeed = fast_fsqrtf(vectorNormSquared(&vCoG)); vectorNormalize(&vCoG, &vCoG); } #endif - + wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 300, 1200), 300, 1200, 0.0f, 1.0f); // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); vHeadingEF.z = 0.0f; From c1c4b8360a9d8fa582265cb4a0126766ad85064e Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 24 Oct 2023 21:04:01 +0900 Subject: [PATCH 093/199] update the imu heading estimation --- src/main/fc/settings.yaml | 4 ++-- src/main/flight/imu.c | 11 +++++++---- src/main/io/gps.c | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9d31a1e231d..fe950eca744 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2324,13 +2324,13 @@ groups: field: w_z_gps_p min: 0 max: 10 - default_value: 0.5 + default_value: 0.4 - name: inav_w_z_gps_v description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors." field: w_z_gps_v min: 0 max: 10 - default_value: 1.0 + default_value: 0.8 - name: inav_w_xy_gps_p description: "Weight of GPS coordinates in estimated UAV position and speed." default_value: 1.0 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 08373b6e5d0..4f55bad7f4b 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -337,7 +337,8 @@ static float imuCalculateMcMagCogWeight(void) wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f); return wCoG; } - +#define COS10DEG 0.984f +#define COS25DEG 0.906f static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; @@ -357,7 +358,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } }; if (magBF && vectorNormSquared(magBF) > 0.01f) { - wMag *= bellCurve((vectorNormSquared(magBF) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); //TODO check if 1024 is correct + wMag *= bellCurve((fast_fsqrtf(vectorNormSquared(magBF)) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS); fpVector3_t vMag; // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF). @@ -401,7 +402,8 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcMagCogWeight(); //scale accroading to multirotor`s tilt angle - wCoG *= scaleRangef(constrainf(vForward.z, 0.98f, 0.90f), 0.98f, 0.90f, 0.0f, 1.0f); //cos(10deg) and cos(25deg) + wCoG *= scaleRangef(constrainf(vForward.z, COS10DEG, COS25DEG), COS10DEG, COS25DEG, 0.0f, 1.0f); + //for inverted flying, wCoG is lowered by imuCalculateMcMagCogWeight } fpVector3_t vHeadingEF; @@ -839,6 +841,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) acc.accADCf[Z] = 0.0f; } } + bool isImuReady(void) { @@ -847,7 +850,7 @@ bool isImuReady(void) bool isImuHeadingValid(void) { - return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING_LEGACY) && gpsHeadingInitialized); + return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || gpsHeadingInitialized; } float calculateCosTiltAngle(void) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 0fae3a7f318..87999e594a0 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -453,7 +453,7 @@ bool isGPSHealthy(void) bool isGPSHeadingValid(void) { - return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300; + return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 500; } #endif From 1c5a42cf4e0968faf4291b6ceb904e6ae8b175aa Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 24 Oct 2023 22:45:11 +0900 Subject: [PATCH 094/199] gps baro mag mix --- src/main/drivers/compass/compass_fake.c | 2 +- src/main/fc/settings.yaml | 5 +++ src/main/flight/imu.c | 55 +++++++++++-------------- src/main/flight/imu.h | 1 + src/main/io/gps.c | 2 +- src/main/target/SITL/sim/realFlight.c | 6 +-- src/main/target/SITL/sim/xplane.c | 6 +-- 7 files changed, 38 insertions(+), 39 deletions(-) diff --git a/src/main/drivers/compass/compass_fake.c b/src/main/drivers/compass/compass_fake.c index a95b3297a5f..2ef8692fd2d 100644 --- a/src/main/drivers/compass/compass_fake.c +++ b/src/main/drivers/compass/compass_fake.c @@ -37,7 +37,7 @@ static bool fakeMagInit(magDev_t *magDev) { UNUSED(magDev); // initially point north - fakeMagData[X] = 4096; + fakeMagData[X] = 1024; fakeMagData[Y] = 0; fakeMagData[Z] = 0; return true; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fe950eca744..55ebd811b0f 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1455,6 +1455,11 @@ groups: default_value: ADAPTIVE field: inertia_comp_method table: imu_inertia_comp_method + - name: ahrs_use_mag_no_gps + description: "Arhs will only use mag when gps is still avaliable" + default_value: OFF + field: use_mag_no_gps + type: bool - name: PG_ARMING_CONFIG type: armingConfig_t diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 4f55bad7f4b..9425218707f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -125,7 +125,8 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .acc_ignore_rate = SETTING_AHRS_ACC_IGNORE_RATE_DEFAULT, .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT, .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT, - .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT + .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT, + .use_mag_no_gps = SETTING_AHRS_USE_MAG_NO_GPS_DEFAULT ); STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) @@ -337,7 +338,7 @@ static float imuCalculateMcMagCogWeight(void) wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f); return wCoG; } -#define COS10DEG 0.984f +#define COS5DEG 0.996f #define COS25DEG 0.906f static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) { @@ -402,7 +403,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcMagCogWeight(); //scale accroading to multirotor`s tilt angle - wCoG *= scaleRangef(constrainf(vForward.z, COS10DEG, COS25DEG), COS10DEG, COS25DEG, 0.0f, 1.0f); + wCoG *= scaleRangef(constrainf(vForward.z, COS5DEG, COS25DEG), COS5DEG, COS25DEG, 0.0f, 1.0f); //for inverted flying, wCoG is lowered by imuCalculateMcMagCogWeight } fpVector3_t vHeadingEF; @@ -429,7 +430,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vectorNormalize(&vCoG, &vCoG); } #endif - wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 300, 1200), 300, 1200, 0.0f, 1.0f); + wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1500), 400, 1500, 0.0f, 1.0f); // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); vHeadingEF.z = 0.0f; @@ -706,37 +707,29 @@ static void imuCalculateEstimatedAttitude(float dT) bool useMag = false; bool useCOG = false; #if defined(USE_GPS) - if (STATE(FIXED_WING_LEGACY) || STATE(MULTIROTOR)) { - bool canUseCOG = isGPSHeadingValid(); + bool canUseCOG = isGPSHeadingValid(); - // Use compass (if available) - if (canUseMAG) { - useMag = true; - gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails + // Use compass (if available) + if (canUseMAG) { + useMag = true; + gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails + } + // Use GPS (if available) + if (canUseCOG && (!(imuConfig()->use_mag_no_gps && useMag))) { + if (gpsHeadingInitialized) { + courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); + useCOG = true; } - // Use GPS (if available) - if (canUseCOG) { - if (gpsHeadingInitialized) { - courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); - useCOG = true; - } - else if (!canUseMAG) { - // Re-initialize quaternion from known Roll, Pitch and GPS heading - imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse); - gpsHeadingInitialized = true; + else if (!canUseMAG) { + // Re-initialize quaternion from known Roll, Pitch and GPS heading + imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse); + gpsHeadingInitialized = true; - // Force reset of heading hold target - resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); - } - } else if (!ARMING_FLAG(ARMED)) { - gpsHeadingInitialized = false; - } - } - else { - // other platform type don't use GPS heading - if (canUseMAG) { - useMag = true; + // Force reset of heading hold target + resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); } + } else if (!ARMING_FLAG(ARMED)) { + gpsHeadingInitialized = false; } imuCalculateFilters(dT); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index a3b87e67360..24904b98ba6 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -54,6 +54,7 @@ typedef struct imuConfig_s { uint8_t acc_ignore_slope; uint8_t gps_yaw_windcomp; uint8_t inertia_comp_method; + uint8_t use_mag_no_gps; } imuConfig_t; PG_DECLARE(imuConfig_t, imuConfig); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 87999e594a0..f71abe897d4 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -453,7 +453,7 @@ bool isGPSHealthy(void) bool isGPSHeadingValid(void) { - return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 500; + return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 400; } #endif diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index 23e77bcde04..05ee701ddd8 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -399,9 +399,9 @@ static void exchangeData(void) computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav); transformVectorEarthToBody(&north, &quat); fakeMagSet( - constrainToInt16(north.x * 16000.0f), - constrainToInt16(north.y * 16000.0f), - constrainToInt16(north.z * 16000.0f) + constrainToInt16(north.x * 1024.0f), + constrainToInt16(north.y * 1024.0f), + constrainToInt16(north.z * 1024.0f) ); free(rfValues.m_currentAircraftStatus); diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c index 44089117d2e..d629c2130e3 100644 --- a/src/main/target/SITL/sim/xplane.c +++ b/src/main/target/SITL/sim/xplane.c @@ -421,9 +421,9 @@ static void* listenWorker(void* arg) computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav); transformVectorEarthToBody(&north, &quat); fakeMagSet( - constrainToInt16(north.x * 16000.0f), - constrainToInt16(north.y * 16000.0f), - constrainToInt16(north.z * 16000.0f) + constrainToInt16(north.x * 1024.0f), + constrainToInt16(north.y * 1024.0f), + constrainToInt16(north.z * 1024.0f) ); if (!initalized) { From cd0206a48f6a278f141e89f922e3e182c55518d3 Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 24 Oct 2023 22:48:46 +0900 Subject: [PATCH 095/199] gps baro mag mix --- src/main/fc/settings.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 55ebd811b0f..237cf99cfdd 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2323,7 +2323,7 @@ groups: field: w_z_baro_p min: 0 max: 10 - default_value: 0.35 + default_value: 0.4 - name: inav_w_z_gps_p description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors." field: w_z_gps_p @@ -2335,7 +2335,7 @@ groups: field: w_z_gps_v min: 0 max: 10 - default_value: 0.8 + default_value: 0.6 - name: inav_w_xy_gps_p description: "Weight of GPS coordinates in estimated UAV position and speed." default_value: 1.0 From 4af73a7c85648eb3ba7b14856e11cebfd4dcde32 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 25 Oct 2023 02:52:49 +0900 Subject: [PATCH 096/199] bug fixes on mag gps mix --- src/main/flight/imu.c | 28 ++++++++++++++------------- src/main/io/gps_fake.c | 1 + src/main/target/SITL/sim/realFlight.c | 16 +++++++-------- 3 files changed, 24 insertions(+), 21 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 9425218707f..3d0ce791f2a 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -77,7 +77,9 @@ #define SPIN_RATE_LIMIT 20 #define MAX_ACC_NEARNESS 0.2 // 20% or G error soft-accepted (0.8-1.2G) -#define MAX_MAG_NEARNESS 0.2 // 20% or magnetic field error soft-accepted (0.8-1.2) +#define MAX_MAG_NEARNESS 0.25 // 25% or magnetic field error soft-accepted (0.75-1.25) +#define COS5DEG 0.996f +#define COS20DEG 0.940f #define IMU_ROTATION_LPF 3 // Hz FASTRAM fpVector3_t imuMeasuredAccelBF; FASTRAM fpVector3_t imuMeasuredRotationBF; @@ -330,7 +332,7 @@ bool isGPSTrustworthy(void) return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6; } -static float imuCalculateMcMagCogWeight(void) +static float imuCalculateMcCogWeight(void) { float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF); float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered)); @@ -338,8 +340,7 @@ static float imuCalculateMcMagCogWeight(void) wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f); return wCoG; } -#define COS5DEG 0.996f -#define COS25DEG 0.906f + static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; @@ -394,18 +395,11 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe if (useCOG) { fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } }; //vForward as trust vector - if STATE(MULTIROTOR){ + if (STATE(MULTIROTOR)){ vForward.z = 1.0f; }else{ vForward.x = 1.0f; } - if (STATE(MULTIROTOR)){ - //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading - wCoG *= imuCalculateMcMagCogWeight(); - //scale accroading to multirotor`s tilt angle - wCoG *= scaleRangef(constrainf(vForward.z, COS5DEG, COS25DEG), COS5DEG, COS25DEG, 0.0f, 1.0f); - //for inverted flying, wCoG is lowered by imuCalculateMcMagCogWeight - } fpVector3_t vHeadingEF; // Use raw heading error (from GPS or whatever else) @@ -430,9 +424,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vectorNormalize(&vCoG, &vCoG); } #endif - wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1500), 400, 1500, 0.0f, 1.0f); + wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1200), 400, 1200, 0.0f, 1.0f); // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); + + if (STATE(MULTIROTOR)){ + //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading + wCoG *= imuCalculateMcCogWeight(); + //scale accroading to multirotor`s tilt angle + wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS5DEG), COS20DEG, COS5DEG, 1.0f, 0.0f); + //for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed + } vHeadingEF.z = 0.0f; // We zeroed out vHeadingEF.z - make sure the whole vector didn't go to zero diff --git a/src/main/io/gps_fake.c b/src/main/io/gps_fake.c index 6d574893436..5b0de8c50b5 100644 --- a/src/main/io/gps_fake.c +++ b/src/main/io/gps_fake.c @@ -31,6 +31,7 @@ #include "platform.h" #include "build/build_config.h" +#include "common/log.h" #if defined(USE_GPS_FAKE) diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index 05ee701ddd8..b4e5b9283ab 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -296,9 +296,9 @@ static void exchangeData(void) //rfValues.m_orientationQuaternion_W = getDoubleFromResponse(response, "m-orientationQuaternion-W"); rfValues.m_aircraftPositionX_MTR = getDoubleFromResponse(response, "m-aircraftPositionX-MTR"); rfValues.m_aircraftPositionY_MTR = getDoubleFromResponse(response, "m-aircraftPositionY-MTR"); - //rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS"); - //rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS"); - //rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS"); + rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS"); + rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS"); + rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS"); //rfValues.m_velocityBodyU_MPS = getDoubleFromResponse(response, "m-velocityBodyU-MPS"); //rfValues.m_velocityBodyV_MPS = getDoubleFromResponse(response, "mm-velocityBodyV-MPS"); //rfValues.m_velocityBodyW_MPS = getDoubleFromResponse(response, "m-velocityBodyW-MPS"); @@ -332,7 +332,7 @@ static void exchangeData(void) float lat, lon; fakeCoords(FAKE_LAT, FAKE_LON, rfValues.m_aircraftPositionX_MTR, -rfValues.m_aircraftPositionY_MTR, &lat, &lon); - int16_t course = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10); + int16_t course = (int16_t)roundf(RADIANS_TO_DECIDEGREES(atan2_approx(-rfValues.m_velocityWorldU_MPS,rfValues.m_velocityWorldV_MPS))); int32_t altitude = (int32_t)roundf(rfValues.m_altitudeASL_MTR * 100); gpsFakeSet( GPS_FIX_3D, @@ -342,9 +342,9 @@ static void exchangeData(void) altitude, (int16_t)roundf(rfValues.m_groundspeed_MPS * 100), course, - 0, - 0, - 0, + 0,//(int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //not sure about the direction + 0,//(int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100), + 0,//(int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100), 0 ); @@ -357,7 +357,7 @@ static void exchangeData(void) const int16_t roll_inav = (int16_t)roundf(rfValues.m_roll_DEG * 10); const int16_t pitch_inav = (int16_t)roundf(-rfValues.m_inclination_DEG * 10); - const int16_t yaw_inav = course; + const int16_t yaw_inav = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10); if (!useImu) { imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav); imuUpdateAttitude(micros()); From 4f518e7f5601d9572f584dff58cf1081fa9b234e Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 25 Oct 2023 03:11:24 +0900 Subject: [PATCH 097/199] minor adjuestments --- src/main/flight/imu.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3d0ce791f2a..2b13880d0ff 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -336,7 +336,7 @@ static float imuCalculateMcCogWeight(void) { float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF); float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered)); - const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope)) * 4.0f; + const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f; wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f); return wCoG; } @@ -424,7 +424,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vectorNormalize(&vCoG, &vCoG); } #endif - wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1200), 400, 1200, 0.0f, 1.0f); + wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); From 9dbbd25f70ffdcd1d3789fc1f22859e8015013b3 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 25 Oct 2023 03:36:18 +0900 Subject: [PATCH 098/199] adjuestable gps yaw weight --- src/main/fc/settings.yaml | 11 ++++++----- src/main/flight/imu.c | 6 ++++-- src/main/flight/imu.h | 2 +- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 237cf99cfdd..5d21793c0d3 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1455,11 +1455,12 @@ groups: default_value: ADAPTIVE field: inertia_comp_method table: imu_inertia_comp_method - - name: ahrs_use_mag_no_gps - description: "Arhs will only use mag when gps is still avaliable" - default_value: OFF - field: use_mag_no_gps - type: bool + - name: ahrs_gps_yaw_weight + description: "Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass" + default_value: 100 + field: gps_yaw_weight + min: 0 + max: 500 - name: PG_ARMING_CONFIG type: armingConfig_t diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 2b13880d0ff..a5eba967501 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -128,7 +128,7 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT, .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT, .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT, - .use_mag_no_gps = SETTING_AHRS_USE_MAG_NO_GPS_DEFAULT + .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT ); STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) @@ -356,6 +356,8 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe if (magBF || useCOG) { float wMag = 1.0f; float wCoG = 1.0f; + if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;} + fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } }; fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } }; @@ -717,7 +719,7 @@ static void imuCalculateEstimatedAttitude(float dT) gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails } // Use GPS (if available) - if (canUseCOG && (!(imuConfig()->use_mag_no_gps && useMag))) { + if (canUseCOG) { if (gpsHeadingInitialized) { courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); useCOG = true; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 24904b98ba6..8afcc50e579 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -54,7 +54,7 @@ typedef struct imuConfig_s { uint8_t acc_ignore_slope; uint8_t gps_yaw_windcomp; uint8_t inertia_comp_method; - uint8_t use_mag_no_gps; + uint16_t gps_yaw_weight; } imuConfig_t; PG_DECLARE(imuConfig_t, imuConfig); From 600d0add2f6cb66c9657a1790160a2c818e3b0de Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 25 Oct 2023 03:59:52 +0900 Subject: [PATCH 099/199] update documents --- docs/Settings.md | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index e00265306b7..46309f86348 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -242,6 +242,16 @@ Inertial Measurement Unit KP Gain for compass measurements --- +### ahrs_gps_yaw_weight + +Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 500 | + +--- + ### ahrs_gps_yaw_windcomp Wind compensation in heading estimation from gps groundcourse(fixed wing only) @@ -1934,21 +1944,21 @@ _// TODO_ ### inav_w_z_baro_p -Weight of barometer measurements in estimated altitude and climb rate +Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors. | Default | Min | Max | | --- | --- | --- | -| 0.35 | 0 | 10 | +| 0.4 | 0 | 10 | --- ### inav_w_z_gps_p -Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes +Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors. | Default | Min | Max | | --- | --- | --- | -| 0.2 | 0 | 10 | +| 0.4 | 0 | 10 | --- @@ -1958,7 +1968,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o | Default | Min | Max | | --- | --- | --- | -| 0.1 | 0 | 10 | +| 0.6 | 0 | 10 | --- From 195a062d27663ae286634cce81d7c3b1b22b983b Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 25 Oct 2023 11:47:15 +0900 Subject: [PATCH 100/199] minor fix --- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_pos_estimator.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5d21793c0d3..63c0d15693e 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2336,7 +2336,7 @@ groups: field: w_z_gps_v min: 0 max: 10 - default_value: 0.6 + default_value: 0.8 - name: inav_w_xy_gps_p description: "Weight of GPS coordinates in estimated UAV position and speed." default_value: 1.0 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index f8473a354f9..e2e6b523045 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -572,7 +572,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) bool correctOK = false; - const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro if difference is too big, baro is probably wrong + const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; //ignore baro if difference is too big, baro is probably wrong //use both baro and gps if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv * 2)) { timeUs_t currentTimeUs = micros(); From 4c4a74cdeffbe64f9ffbec32dd8f9b1cb48d1bb0 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 25 Oct 2023 15:23:33 +0900 Subject: [PATCH 101/199] fix pos estimator acc weight --- .../navigation/navigation_pos_estimator.c | 33 ++++++++++--------- src/main/sensors/acceleration.h | 2 +- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index e2e6b523045..2fccdfa980f 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -373,18 +373,19 @@ static bool gravityCalibrationComplete(void) static void updateIMUEstimationWeight(const float dt) { - bool isAccClipped = accIsClipped(); - - // If accelerometer measurement is clipped - drop the acc weight to zero + static float acc_clip_factor = 1.0f; + // If accelerometer measurement is clipped - drop the acc weight to 0.3 // and gradually restore weight back to 1.0 over time - if (isAccClipped) { - posEstimator.imu.accWeightFactor = 0.0f; + if (accIsClipped()) { + acc_clip_factor = 0.5f; } else { const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT); - posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha; + acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha; } - + // Update accelerometer weight based on vibration levels and clipping + float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),1.0f,4.0f),1.0f,2.0f,1.0f,0.3f); // g/s + posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor; // DEBUG_VIBE[0-3] are used in IMU DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000); } @@ -534,13 +535,12 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) static void estimationPredict(estimationContext_t * ctx) { - const float accWeight = navGetAccelerometerWeight(); /* Prediction step: Z-axis */ if ((ctx->newFlags & EST_Z_VALID)) { posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt; - posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight; - posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight); + posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; + posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; } /* Prediction step: XY-axis */ @@ -551,10 +551,10 @@ static void estimationPredict(estimationContext_t * ctx) // If heading is valid, accelNEU is valid as well. Account for acceleration if (navIsHeadingUsable() && navIsAccelerationUsable()) { - posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight; - posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight; - posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight); - posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight); + posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f; + posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f; + posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt; + posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt; } } } @@ -754,7 +754,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) { ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; } - + // Boost the corrections based on accWeight + const float accWeight = navGetAccelerometerWeight(); + vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight); + vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight); // Apply corrections vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index d81a83a9084..ff643f4a153 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -27,7 +27,7 @@ #define GRAVITY_CMSS 980.665f #define GRAVITY_MSS 9.80665f -#define ACC_CLIPPING_THRESHOLD_G 7.9f +#define ACC_CLIPPING_THRESHOLD_G 15.9f #define ACC_VIBE_FLOOR_FILT_HZ 5.0f #define ACC_VIBE_FILT_HZ 2.0f From 3978947777666e9decbdf9ac36df916e578a746c Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 25 Oct 2023 15:25:00 +0900 Subject: [PATCH 102/199] update docs --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 46309f86348..4007ed1f654 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1968,7 +1968,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o | Default | Min | Max | | --- | --- | --- | -| 0.6 | 0 | 10 | +| 0.8 | 0 | 10 | --- From 8e7cc6bb32c34950d8074c1b47805d986978cd65 Mon Sep 17 00:00:00 2001 From: shota Date: Thu, 26 Oct 2023 00:16:33 +0900 Subject: [PATCH 103/199] fixes display issue in configurator sensors tab --- src/main/fc/fc_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cdaac609baa..c827e07890d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -477,7 +477,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } for (int i = 0; i < 3; i++) { #ifdef USE_MAG - sbufWriteU16(dst, mag.magADC[i]); + sbufWriteU16(dst, lrintf(mag.magADC[i])); #else sbufWriteU16(dst, 0); #endif From 85217c24c6d5bfbd47f15cd13847cae9076e1c28 Mon Sep 17 00:00:00 2001 From: Stefano Della Valle Date: Wed, 25 Oct 2023 19:28:54 +0200 Subject: [PATCH 104/199] Update logic_condition.c Added activation of the compass calibration request --- src/main/programming/logic_condition.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 52e5e909c32..4b63350da2c 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -286,6 +286,13 @@ static int logicConditionCompute( return true; break; +#ifdef USE_MAG + case LOGIC_CONDITION_RESET_MAG_CALIBRATION: + + ENABLE_STATE(CALIBRATE_MAG); + return true; + break; +#endif case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: #if defined(USE_VTX_CONTROL) #if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)) From ec40147e9734d81a52e78b8e9c43d8884df65246 Mon Sep 17 00:00:00 2001 From: Stefano Della Valle Date: Wed, 25 Oct 2023 19:29:53 +0200 Subject: [PATCH 105/199] Update logic_condition.h Added LOGIC_CONDITION_RESET_MAG_CALIBRATION operation --- src/main/programming/logic_condition.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index cb87566f81d..d5ca58e097a 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -81,7 +81,8 @@ typedef enum { LOGIC_CONDITION_TIMER = 49, LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_APPROX_EQUAL = 51, - LOGIC_CONDITION_LAST = 52, + LOGIC_CONDITION_RESET_MAG_CALIBRATION = 52, + LOGIC_CONDITION_LAST = 53, } logicOperation_e; typedef enum logicOperandType_s { From eb6634da67b40cd7cb1860e3fbf5944bc9f6ca7d Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 27 Oct 2023 00:56:26 +0900 Subject: [PATCH 106/199] minor fix on cog yaw estimation add acc vibration to blackbox adjustments in imu cog minor fix on blackbox --- src/main/blackbox/blackbox.c | 5 +++++ src/main/flight/imu.c | 7 ++++--- src/main/sensors/acceleration.c | 3 ++- src/main/sensors/acceleration.h | 1 + 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index cb12a7a8968..ae64a3e35a2 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -310,6 +310,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC}, {"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC}, {"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC}, + {"accVib", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC}, {"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE}, {"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE}, {"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE}, @@ -487,6 +488,7 @@ typedef struct blackboxMainState_s { int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT]; int16_t accADC[XYZ_AXIS_COUNT]; + int16_t accVib; int16_t attitude[XYZ_AXIS_COUNT]; int32_t debug[DEBUG32_VALUE_COUNT]; int16_t motor[MAX_SUPPORTED_MOTORS]; @@ -917,6 +919,7 @@ static void writeIntraframe(void) if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) { blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT); + blackboxWriteUnsignedVB(blackboxCurrent->accVib); } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) { @@ -1182,6 +1185,7 @@ static void writeInterframe(void) if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) { blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT); + blackboxWriteSignedVB(blackboxCurrent->accVib - blackboxLast->accVib); } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) { @@ -1601,6 +1605,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained); } } + blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G); if (STATE(FIXED_WING_LEGACY)) { diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index a5eba967501..afdc86e6bf9 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -49,6 +49,7 @@ #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #if defined(USE_WIND_ESTIMATOR) #include "flight/wind_estimator.h" @@ -78,7 +79,7 @@ #define SPIN_RATE_LIMIT 20 #define MAX_ACC_NEARNESS 0.2 // 20% or G error soft-accepted (0.8-1.2G) #define MAX_MAG_NEARNESS 0.25 // 25% or magnetic field error soft-accepted (0.75-1.25) -#define COS5DEG 0.996f +#define COS10DEG 0.985f #define COS20DEG 0.940f #define IMU_ROTATION_LPF 3 // Hz FASTRAM fpVector3_t imuMeasuredAccelBF; @@ -397,7 +398,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe if (useCOG) { fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } }; //vForward as trust vector - if (STATE(MULTIROTOR)){ + if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){ vForward.z = 1.0f; }else{ vForward.x = 1.0f; @@ -434,7 +435,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcCogWeight(); //scale accroading to multirotor`s tilt angle - wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS5DEG), COS20DEG, COS5DEG, 1.0f, 0.0f); + wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f); //for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed } vHeadingEF.z = 0.0f; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index f897d7e580a..695f49b00b2 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -576,6 +576,7 @@ void accUpdate(void) // calc difference from this sample and 5hz filtered value, square and filter at 2hz const float accDiff = acc.accADCf[axis] - accFloorFilt; acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff); + acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]); } // Filter acceleration @@ -612,7 +613,7 @@ void accGetVibrationLevels(fpVector3_t *accVibeLevels) float accGetVibrationLevel(void) { - return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]); + return acc.accVibe; } uint32_t accGetClipCount(void) diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index ff643f4a153..f3385edc6d5 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -58,6 +58,7 @@ typedef struct acc_s { uint32_t accTargetLooptime; float accADCf[XYZ_AXIS_COUNT]; // acceleration in g float accVibeSq[XYZ_AXIS_COUNT]; + float accVibe; uint32_t accClipCount; bool isClipped; acc_extremes_t extremes[XYZ_AXIS_COUNT]; From b911185a881c03e53c0a30d4b2131444fa7405a0 Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 27 Oct 2023 12:03:25 +0900 Subject: [PATCH 107/199] adjustments --- docs/Settings.md | 10 ---------- src/main/fc/settings.yaml | 5 ----- src/main/navigation/navigation.h | 1 - src/main/navigation/navigation_pos_estimator.c | 9 ++++----- 4 files changed, 4 insertions(+), 21 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ca98e074c29..c1a109f3593 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1922,16 +1922,6 @@ Decay coefficient for estimated velocity when GPS reference for position is lost --- -### inav_w_xyz_acc_p - -_// TODO_ - -| Default | Min | Max | -| --- | --- | --- | -| 1.0 | 0 | 1 | - ---- - ### inav_w_z_baro_p Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 91292198daf..90e7cff250b 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2366,11 +2366,6 @@ groups: field: w_xy_res_v min: 0 max: 10 - - name: inav_w_xyz_acc_p - field: w_xyz_acc_p - min: 0 - max: 1 - default_value: 1.0 - name: inav_w_acc_bias description: "Weight for accelerometer drift estimation" default_value: 0.01 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ecc487cc9e7..57f8a192b41 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -203,7 +203,6 @@ typedef struct positionEstimationConfig_s { float w_xy_res_v; float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. - float w_xyz_acc_p; float max_eph_epv; // Max estimated position error acceptable for estimation (cm) float baro_epv; // Baro position error diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index a18b6abec15..1b561e98810 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -70,8 +70,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT, - .w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT, - .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT, .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT, @@ -389,7 +387,8 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } - +#define ACC_VIB_FACTOR_S 1.0f +#define ACC_VIB_FACTOR_E 3.0f static void updateIMUEstimationWeight(const float dt) { static float acc_clip_factor = 1.0f; @@ -403,7 +402,7 @@ static void updateIMUEstimationWeight(const float dt) acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha; } // Update accelerometer weight based on vibration levels and clipping - float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),1.0f,4.0f),1.0f,2.0f,1.0f,0.3f); // g/s + float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E,1.0f,0.3f); // g/s posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor; // DEBUG_VIBE[0-3] are used in IMU DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000); @@ -411,7 +410,7 @@ static void updateIMUEstimationWeight(const float dt) float navGetAccelerometerWeight(void) { - const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p; + const float accWeightScaled = posEstimator.imu.accWeightFactor; DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000); return accWeightScaled; From cece1d7517952cd8f676ae830dfdb729ceece895 Mon Sep 17 00:00:00 2001 From: Stefano Della Valle Date: Fri, 27 Oct 2023 11:21:55 +0200 Subject: [PATCH 108/199] Update logic_condition.h changed LOGIC_CONDITION_RESET_MAG_CALIBRATION to 53 so solve a conflict with an other PR --- src/main/programming/logic_condition.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index d5ca58e097a..df057745340 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -81,8 +81,8 @@ typedef enum { LOGIC_CONDITION_TIMER = 49, LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_APPROX_EQUAL = 51, - LOGIC_CONDITION_RESET_MAG_CALIBRATION = 52, - LOGIC_CONDITION_LAST = 53, + LOGIC_CONDITION_RESET_MAG_CALIBRATION = 53, + LOGIC_CONDITION_LAST = 54, } logicOperation_e; typedef enum logicOperandType_s { From a0f353eb49e8d549443abfd3b6155cff69ab9d4e Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 29 Oct 2023 17:04:56 +0900 Subject: [PATCH 109/199] separate the nav climb rate settings for fw and mc --- src/main/cms/cms_menu_navigation.c | 5 ++-- src/main/fc/fc_msp.c | 12 +++++--- src/main/fc/settings.yaml | 30 ++++++++++++-------- src/main/navigation/navigation.c | 5 ++-- src/main/navigation/navigation.h | 5 ++-- src/main/navigation/navigation_fixedwing.c | 2 +- src/main/navigation/navigation_multicopter.c | 16 +++++------ 7 files changed, 44 insertions(+), 31 deletions(-) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index a54843afc40..f5b12301028 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -45,8 +45,9 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] = OSD_SETTING_ENTRY("MC NAV SPEED", SETTING_NAV_AUTO_SPEED), OSD_SETTING_ENTRY("MC MAX NAV SPEED", SETTING_NAV_MAX_AUTO_SPEED), OSD_SETTING_ENTRY("MAX CRUISE SPEED", SETTING_NAV_MANUAL_SPEED), - OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE), - OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE), + OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_MC_AUTO_CLIMB_RATE), + OSD_SETTING_ENTRY("MAX MC AH CLIMB RATE", SETTING_NAV_MC_MANUAL_CLIMB_RATE), + OSD_SETTING_ENTRY("MAX FW AH CLIMB RATE", SETTING_NAV_FW_MANUAL_CLIMB_RATE), OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE), OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE), OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR), diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f504ffb0598..bc260a64935 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1310,9 +1310,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_NAV_POSHOLD: sbufWriteU8(dst, navConfig()->general.flags.user_control_mode); sbufWriteU16(dst, navConfig()->general.max_auto_speed); - sbufWriteU16(dst, navConfig()->general.max_auto_climb_rate); + sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate); sbufWriteU16(dst, navConfig()->general.max_manual_speed); - sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate); + sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate); sbufWriteU8(dst, navConfig()->mc.max_bank_angle); sbufWriteU8(dst, navConfig()->mc.althold_throttle_type); sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle); @@ -2321,9 +2321,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize == 13) { navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); navConfigMutable()->general.max_auto_speed = sbufReadU16(src); - navConfigMutable()->general.max_auto_climb_rate = sbufReadU16(src); + navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); navConfigMutable()->general.max_manual_speed = sbufReadU16(src); - navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src); + if (mixerConfig()->platformType != PLATFORM_AIRPLANE) { + navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src); + }else{ + navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src); + } navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src); currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b1bb61ed091..eb819404267 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2489,24 +2489,12 @@ groups: field: general.max_auto_speed min: 10 max: 2000 - - name: nav_auto_climb_rate - description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" - default_value: 500 - field: general.max_auto_climb_rate - min: 10 - max: 2000 - name: nav_manual_speed description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]" default_value: 500 field: general.max_manual_speed min: 10 max: 2000 - - name: nav_manual_climb_rate - description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" - default_value: 200 - field: general.max_manual_climb_rate - min: 10 - max: 2000 - name: nav_land_minalt_vspd description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]" default_value: 50 @@ -2677,6 +2665,18 @@ groups: field: mc.max_bank_angle min: 15 max: 45 + - name: nav_mc_auto_climb_rate + description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" + default_value: 500 + field: mc.max_auto_climb_rate + min: 10 + max: 2000 + - name: nav_mc_manual_climb_rate + description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" + default_value: 200 + field: mc.max_manual_climb_rate + min: 10 + max: 2000 - name: nav_auto_disarm_delay description: "Delay before craft disarms when `nav_disarm_on_landing` is set (ms)" default_value: 1000 @@ -2762,6 +2762,12 @@ groups: field: fw.max_bank_angle min: 5 max: 80 + - name: nav_fw_manual_climb_rate + description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" + default_value: 300 + field: fw.max_manual_climb_rate + min: 10 + max: 2500 - name: nav_fw_climb_angle description: "Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit" default_value: 20 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cb12579b83d..0eccbc091db 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -133,9 +133,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h) .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes - .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, - .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT, .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s @@ -160,6 +158,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // MC-specific .mc = { .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees + .max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s + .max_manual_climb_rate = SETTING_NAV_MC_MANUAL_CLIMB_RATE_DEFAULT, #ifdef USE_MR_BRAKING_MODE .braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s @@ -181,6 +181,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // Fixed wing .fw = { .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees + .max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ecc487cc9e7..880910bd850 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -246,9 +246,7 @@ typedef struct navConfig_s { bool waypoint_load_on_boot; // load waypoints automatically during boot uint16_t auto_speed; // autonomous navigation speed cm/sec uint16_t max_auto_speed; // maximum allowed autonomous navigation speed cm/sec - uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec uint16_t max_manual_speed; // manual velocity control max horizontal speed - uint16_t max_manual_climb_rate; // manual velocity control max vertical speed uint16_t land_minalt_vspd; // Final RTH landing descent rate under minalt uint16_t land_maxalt_vspd; // RTH landing descent rate target at maxalt uint16_t land_slowdown_minalt; // Altitude to stop lowering descent rate during RTH descend @@ -272,6 +270,8 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // multicopter max banking angle (deg) + uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec + uint16_t max_manual_climb_rate; // manual velocity control max vertical speed #ifdef USE_MR_BRAKING_MODE uint16_t braking_speed_threshold; // above this speed braking routine might kick in @@ -292,6 +292,7 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // Fixed wing max banking angle (deg) + uint16_t max_manual_climb_rate; // manual velocity control max vertical speed uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg) uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e6084e0972e..8fcbce4b557 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -116,7 +116,7 @@ bool adjustFixedWingAltitudeFromRCInput(void) if (rcAdjustment) { // set velocity proportional to stick movement - float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband); + float rcClimbRate = -rcAdjustment * navConfig()->fw.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband); updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2264d842014..02e3edbb910 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -77,9 +77,9 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) float vel_max_z = 0.0f; if (posControl.flags.isAdjustingAltitude) { - vel_max_z = navConfig()->general.max_manual_climb_rate; + vel_max_z = navConfig()->mc.max_manual_climb_rate; } else { - vel_max_z = navConfig()->general.max_auto_climb_rate; + vel_max_z = navConfig()->mc.max_auto_climb_rate; } targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); @@ -151,11 +151,11 @@ bool adjustMulticopterAltitudeFromRCInput(void) // Make sure we can satisfy max_manual_climb_rate in both up and down directions if (rcThrottleAdjustment > 0) { // Scaling from altHoldThrottleRCZero to maxthrottle - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); + rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); } else { // Scaling from minthrottle to altHoldThrottleRCZero - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband); + rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband); } updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); @@ -221,11 +221,11 @@ void resetMulticopterAltitudeController(void) const float maxSpeed = getActiveSpeed(); nav_speed_up = maxSpeed; nav_accel_z = maxSpeed; - nav_speed_down = navConfig()->general.max_auto_climb_rate; + nav_speed_down = navConfig()->mc.max_auto_climb_rate; } else { nav_speed_up = navConfig()->general.max_manual_speed; nav_accel_z = navConfig()->general.max_manual_speed; - nav_speed_down = navConfig()->general.max_manual_climb_rate; + nav_speed_down = navConfig()->mc.max_manual_climb_rate; } sqrtControllerInit( @@ -252,8 +252,8 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) if (prepareForTakeoffOnReset) { const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity(); - posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate; - posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP); + posControl.desiredState.vel.z = -navConfig()->mc.max_manual_climb_rate; + posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->mc.max_manual_climb_rate / posControl.pids.pos[Z].param.kP); posControl.pids.vel[Z].integrator = -500.0f; pt1FilterReset(&altholdThrottleFilterState, -500.0f); prepareForTakeoffOnReset = false; From edf9ccac236bbe1dbf0125926505d775c4283059 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 29 Oct 2023 17:22:48 +0900 Subject: [PATCH 110/199] update documents --- docs/Settings.md | 50 +++++++++++++++++++++++++++++------------------- 1 file changed, 30 insertions(+), 20 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 15ce74ef384..1eeafb52112 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2752,16 +2752,6 @@ Craft name --- -### nav_auto_climb_rate - -Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] - -| Default | Min | Max | -| --- | --- | --- | -| 500 | 10 | 2000 | - ---- - ### nav_auto_disarm_delay Delay before craft disarms when `nav_disarm_on_landing` is set (ms) @@ -3082,6 +3072,16 @@ PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] --- +### nav_fw_manual_climb_rate + +Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 300 | 10 | 2500 | + +--- + ### nav_fw_max_thr Maximum throttle for flying wing in GPS assisted modes @@ -3352,16 +3352,6 @@ Allows immediate landing detection based on G bump at touchdown when set to ON. --- -### nav_manual_climb_rate - -Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] - -| Default | Min | Max | -| --- | --- | --- | -| 200 | 10 | 2000 | - ---- - ### nav_manual_speed Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] @@ -3412,6 +3402,16 @@ If set to STICK the FC remembers the throttle stick position when enabling ALTHO --- +### nav_mc_auto_climb_rate + +Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 10 | 2000 | + +--- + ### nav_mc_bank_angle Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude @@ -3522,6 +3522,16 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx --- +### nav_mc_manual_climb_rate + +Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 10 | 2000 | + +--- + ### nav_mc_pos_deceleration_time Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting From 13d8fbab909b8fa6241daf84a3f8e53c108ae4c6 Mon Sep 17 00:00:00 2001 From: MrD-RC Date: Mon, 30 Oct 2023 13:24:55 +0000 Subject: [PATCH 111/199] Move minimum ground speed to a changeable parameter --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 6 ++++++ src/main/navigation/navigation_fixedwing.c | 7 +++---- src/main/sensors/battery.c | 2 +- src/main/sensors/battery_config_structs.h | 2 ++ 5 files changed, 22 insertions(+), 5 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index e00265306b7..61dc113d7a0 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3692,6 +3692,16 @@ When ON, NAV engine will slow down when switching to the next waypoint. This pri --- +### nav_min_ground_speed + +Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s. + +| Default | Min | Max | +| --- | --- | --- | +| 7 | 6 | 45 | + +--- + ### nav_min_rth_distance Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 87daf753cf2..73d16d8b141 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1043,6 +1043,12 @@ groups: default_value: 1000 min: PWM_RANGE_MIN max: PWM_RANGE_MAX + - name: nav_min_ground_speed + description: "Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s." + default_value: 7 + field: nav.min_ground_speed + min: 6 + max: 45 - name: nav_mc_hover_thr description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering." default_value: 1300 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e6084e0972e..148617a9a49 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -60,9 +60,8 @@ #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f -// If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind +// If we are going slower than the minimum ground speed (currentBatteryProfile->nav.min_ground_speed) - boost throttle to fight against the wind #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN 1.5f -#define NAV_FW_MIN_VEL_SPEED_BOOST 700.0f // 7 m/s // If this is enabled navigation won't be applied if velocity is below 3 m/s //#define NAV_FW_LIMIT_MIN_FLY_VELOCITY @@ -552,10 +551,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) previousTimePositionUpdate = currentTimeUs; if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); + float velThrottleBoost = ((currentBatteryProfile->nav.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); // If we are in the deadband of 50cm/s - don't update speed boost - if (fabsf(posControl.actualState.velXY - NAV_FW_MIN_VEL_SPEED_BOOST) > 50) { + if (fabsf(posControl.actualState.velXY - (currentBatteryProfile->nav.min_ground_speed * 100.0f)) > 50) { throttleSpeedAdjustment += velThrottleBoost; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 3eaeecc5f08..8f21c553503 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -134,6 +134,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off. .nav = { + .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, .mc = { .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT, @@ -147,7 +148,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT, .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP } - }, #if defined(USE_POWER_LIMITS) diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h index 81eafef44ad..183a1fd2703 100644 --- a/src/main/sensors/battery_config_structs.h +++ b/src/main/sensors/battery_config_structs.h @@ -123,6 +123,8 @@ typedef struct batteryProfile_s { uint16_t launch_throttle; // Launch throttle } fw; + uint8_t min_ground_speed; // Minimum navigation ground speed [m/s] + } nav; #if defined(USE_POWER_LIMITS) From 835a2240a4d016db22a3ef4fe9cc4cd3abb5adc6 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 30 Oct 2023 13:34:17 +0000 Subject: [PATCH 112/199] Update battery.c Pump PG --- src/main/sensors/battery.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 8f21c553503..3273c2d0578 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -97,7 +97,7 @@ static int32_t mWhDrawn = 0; // energy (milliWatt hours) draw batteryState_e batteryState; const batteryProfile_t *currentBatteryProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 3); void pgResetFn_batteryProfiles(batteryProfile_t *instance) { From 9aa64c66cf4a175f16457f5983180a5218f0aa97 Mon Sep 17 00:00:00 2001 From: Julio Cesar Date: Mon, 30 Oct 2023 20:32:50 -0300 Subject: [PATCH 113/199] create .c and .h archives to RTH TrackBack --- .vscode/settings.json | 4 +- src/main/CMakeLists.txt | 2 + src/main/common/maths.h | 6 +- src/main/fc/multifunction.h | 2 + src/main/navigation/navigation.c | 172 ++------------------- src/main/navigation/navigation.h | 4 + src/main/navigation/navigation_private.h | 8 - src/main/navigation/rth_trackback.c | 184 +++++++++++++++++++++++ src/main/navigation/rth_trackback.h | 42 ++++++ 9 files changed, 254 insertions(+), 170 deletions(-) create mode 100644 src/main/navigation/rth_trackback.c create mode 100644 src/main/navigation/rth_trackback.h diff --git a/.vscode/settings.json b/.vscode/settings.json index 2cece3ee127..9faa14c8f69 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,7 +2,9 @@ "files.associations": { "chrono": "c", "cmath": "c", - "ranges": "c" + "ranges": "c", + "navigation.h": "c", + "rth_trackback.h": "c" }, "editor.tabSize": 4, "editor.insertSpaces": true, diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 313de401868..0ec295448d2 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -556,6 +556,8 @@ main_sources(COMMON_SRC navigation/navigation_rover_boat.c navigation/sqrt_controller.c navigation/sqrt_controller.h + navigation/rth_trackback.c + navigation/rth_trackback.h sensors/barometer.c sensors/barometer.h diff --git a/src/main/common/maths.h b/src/main/common/maths.h index ff0505fb531..6c9e40d6f07 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -36,7 +36,7 @@ #define RAD (M_PIf / 180.0f) #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) -#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f) +#define CENTIDEGREES_TO_DEGREES(angle) ((angle) * 0.01f) #define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f) #define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10) @@ -54,11 +54,11 @@ #define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD) #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) -#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD) +#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD) #define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) #define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) -#define CENTIMETERS_TO_METERS(cm) (cm / 100.0f) +#define CENTIMETERS_TO_METERS(cm) (cm * 0.01f) #define METERS_TO_CENTIMETERS(m) (m * 100) diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h index b5c1e1d9d69..93265ba1200 100644 --- a/src/main/fc/multifunction.h +++ b/src/main/fc/multifunction.h @@ -24,6 +24,8 @@ #pragma once +#include + #ifdef USE_MULTI_FUNCTIONS extern uint8_t multiFunctionFlags; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 69a9c0479a9..eed1dcffc1b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -55,6 +55,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "navigation/rth_trackback.h" #include "rx/rx.h" @@ -254,10 +255,8 @@ static void resetJumpCounter(void); static void clearJumpCounters(void); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); -static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos); void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); -static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing); bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); @@ -269,11 +268,6 @@ bool validateRTHSanityChecker(void); void updateHomePosition(void); bool abortLaunchAllowed(void); -static bool rthAltControlStickOverrideCheck(unsigned axis); - -static void updateRthTrackback(bool forceSaveTrackPoint); -static fpVector3_t * rthGetTrackbackPos(void); - /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState); @@ -1250,16 +1244,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING - } - else { - // Switch to RTH trackback - bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || - (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated); - - if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) { - updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference + } else { + if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) { + rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference posControl.flags.rthTrackbackActive = true; - calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos()); + calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK; } @@ -1374,36 +1363,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (posControl.flags.estPosStatus >= EST_USABLE) { - const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100; -#ifdef USE_MULTI_FUNCTIONS - const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK); -#else - const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL); -#endif - const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance || - (overrideTrackback && !posControl.flags.forcedRTHActivated); - - if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) { - posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1; - posControl.flags.rthTrackbackActive = false; - return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point - } - - if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { - posControl.activeRthTBPointIndex--; - - if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) { - posControl.activeRthTBPointIndex = NAV_RTH_TRACKBACK_POINTS - 1; - } - calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos()); - - if (posControl.activeRthTBPointIndex - posControl.rthTBWrapAroundCounter == 0) { - posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1; - } - } else { - setDesiredPosition(rthGetTrackbackPos(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); - } + if (rthTrackBackSetNewPosition()) { + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; } return NAV_FSM_EVENT_NONE; @@ -2379,7 +2340,7 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos) * Check if waypoint is/was reached. * waypointBearing stores initial bearing to waypoint *-----------------------------------------------------------*/ -static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing) +bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing) { posControl.wpDistance = calculateDistanceToDestination(waypointPos); @@ -2716,12 +2677,13 @@ void updateHomePosition(void) * Climb First override limited to Fixed Wing only * Roll also cancels RTH trackback on Fixed Wing and Multirotor *-----------------------------------------------------------*/ -static bool rthAltControlStickOverrideCheck(unsigned axis) +bool rthAltControlStickOverrideCheck(uint8_t axis) { if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated || (axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) { return false; } + static timeMs_t rthOverrideStickHoldStartTime[2]; if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { @@ -2760,110 +2722,6 @@ static bool rthAltControlStickOverrideCheck(unsigned axis) return false; } -/* -------------------------------------------------------------------------------- - * == RTH Trackback == - * Saves track during flight which is used during RTH to back track - * along arrival route rather than immediately heading directly toward home. - * Max desired trackback distance set by user or limited by number of available points. - * Reverts to normal RTH heading direct to home when end of track reached. - * Trackpoints logged with precedence for course/altitude changes. Distance based changes - * only logged if no course/altitude changes logged over an extended distance. - * Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold). - * --------------------------------------------------------------------------------- */ - static void updateRthTrackback(bool forceSaveTrackPoint) -{ - static bool suspendTracking = false; - bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE)); - if (!fwLoiterIsActive && suspendTracking) { - suspendTracking = false; - } - - if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) { - return; - } - - // Record trackback points based on significant change in course/altitude until - // points limit reached. Overwrite older points from then on. - if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) { - static int32_t previousTBTripDist; // cm - static int16_t previousTBCourse; // degrees - static int16_t previousTBAltitude; // meters - static uint8_t distanceCounter = 0; - bool saveTrackpoint = forceSaveTrackPoint; - bool GPSCourseIsValid = isGPSHeadingValid(); - - // start recording when some distance from home, 50m seems reasonable. - if (posControl.activeRthTBPointIndex < 0) { - saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50); - - previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog); - previousTBTripDist = posControl.totalTripDistance; - } else { - // Minimum distance increment between course change track points when GPS course valid - set to 10m - const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(10); - - // Altitude change - if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > 10) { // meters - saveTrackpoint = true; - } else if (distanceIncrement && GPSCourseIsValid) { - // Course change - set to 45 degrees - if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) { - saveTrackpoint = true; - } else if (distanceCounter >= 9) { - // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change - // and deviation from projected course path > 20m - float distToPrevPoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]); - - fpVector3_t virtualCoursePoint; - virtualCoursePoint.x = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse)); - virtualCoursePoint.y = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse)); - - saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(20); - } - distanceCounter++; - previousTBTripDist = posControl.totalTripDistance; - } else if (!GPSCourseIsValid) { - // if no reliable course revert to basic distance logging based on direct distance from last point - set to 20m - saveTrackpoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]) > METERS_TO_CENTIMETERS(20); - previousTBTripDist = posControl.totalTripDistance; - } - - // Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter. - if (distanceCounter && fwLoiterIsActive) { - saveTrackpoint = suspendTracking = true; - } - } - - // when trackpoint store full, overwrite from start of store using 'rthTBWrapAroundCounter' to track overwrite position - if (saveTrackpoint) { - if (posControl.activeRthTBPointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) { // wraparound to start - posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = 0; - } else { - posControl.activeRthTBPointIndex++; - if (posControl.rthTBWrapAroundCounter > -1) { // track wraparound overwrite position after store first filled - posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex; - } - } - posControl.rthTBPointsList[posControl.activeRthTBPointIndex] = posControl.actualState.abs.pos; - - posControl.rthTBLastSavedIndex = posControl.activeRthTBPointIndex; - previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z); - previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse; - distanceCounter = 0; - } - } -} - -static fpVector3_t * rthGetTrackbackPos(void) -{ - // ensure trackback altitude never lower than altitude of start point - if (posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z < posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z) { - posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z = posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z; - } - - return &posControl.rthTBPointsList[posControl.activeRthTBPointIndex]; -} - /*----------------------------------------------------------- * Update flight statistics *-----------------------------------------------------------*/ @@ -3519,7 +3377,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv); } -static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos) +void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos) { // Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints) if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) { @@ -3612,7 +3470,7 @@ bool isWaypointNavTrackingActive(void) // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP. // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called) - return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex); + return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex); } /*----------------------------------------------------------- @@ -3669,9 +3527,7 @@ void applyWaypointNavigationAndAltitudeHold(void) // ensure WP missions always restart from first waypoint after disarm posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback - posControl.activeRthTBPointIndex = -1; - posControl.flags.rthTrackbackActive = false; - posControl.rthTBWrapAroundCounter = -1; + resetRthTrackBack(); return; } @@ -4199,7 +4055,7 @@ void updateWaypointsAndNavigationMode(void) updateWpMissionPlanner(); // Update RTH trackback - updateRthTrackback(false); + rthTrackBackUpdate(false); //Update Blackbox data navCurrentState = (int16_t)posControl.navPersistentId; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 3a9223ff639..34afdb1441a 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -574,6 +574,9 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units // Select absolute or relative altitude based on WP mission flag setting geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag); +void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos); +bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing); + /* Distance/bearing calculation */ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); // NOT USED uint32_t distanceToFirstWP(void); @@ -624,6 +627,7 @@ bool isEstimatedAglTrusted(void); void checkManualEmergencyLandingControl(bool forcedActivation); float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue); +bool rthAltControlStickOverrideCheck(uint8_t axis); /* Returns the heading recorded when home position was acquired. * Note that the navigation system uses deg*100 as unit and angles diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index c408f109c9b..3b2900d45d9 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -45,8 +45,6 @@ #define MC_LAND_DESCEND_THROTTLE 40 // RC pwm units (us) #define MC_LAND_SAFE_SURFACE 5.0f // cm -#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points - #define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); @@ -426,12 +424,6 @@ typedef struct { timeMs_t wpReachedTime; // Time the waypoint was reached bool wpAltitudeReached; // WP altitude achieved - /* RTH Trackback */ - fpVector3_t rthTBPointsList[NAV_RTH_TRACKBACK_POINTS]; - int8_t rthTBLastSavedIndex; // last trackback point index saved - int8_t activeRthTBPointIndex; - int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position - /* Internals & statistics */ int16_t rcAdjustment[4]; float totalTripDistance; diff --git a/src/main/navigation/rth_trackback.c b/src/main/navigation/rth_trackback.c new file mode 100644 index 00000000000..a5bbd08d83c --- /dev/null +++ b/src/main/navigation/rth_trackback.c @@ -0,0 +1,184 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +/* -------------------------------------------------------------------------------- + * == RTH Trackback == + * Saves track during flight which is used during RTH to back track + * along arrival route rather than immediately heading directly toward home. + * Max desired trackback distance set by user or limited by number of available points. + * Reverts to normal RTH heading direct to home when end of track reached. + * Trackpoints logged with precedence for course/altitude changes. Distance based changes + * only logged if no course/altitude changes logged over an extended distance. + * Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold). + * --------------------------------------------------------------------------------- */ + +#include "platform.h" + +#include "fc/multifunction.h" +#include "fc/rc_controls.h" + +#include "navigation/rth_trackback.h" +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +rth_trackback_t rth_trackback; + +bool rthTrackBackIsActive(void) +{ + return navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated); +} + +void rthTrackBackUpdate(bool forceSaveTrackPoint) +{ + static bool suspendTracking = false; + bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE)); + + if (!fwLoiterIsActive && suspendTracking) { + suspendTracking = false; + } + + if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) { + return; + } + + // Record trackback points based on significant change in course/altitude until + // points limit reached. Overwrite older points from then on. + if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) { + static int32_t previousTBTripDist; // cm + static int16_t previousTBCourse; // degrees + static int16_t previousTBAltitude; // meters + static uint8_t distanceCounter = 0; + bool saveTrackpoint = forceSaveTrackPoint; + bool GPSCourseIsValid = isGPSHeadingValid(); + + // Start recording when some distance from home + if (rth_trackback.activePointIndex < 0) { + saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_DIST_TO_START); + previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog); + previousTBTripDist = posControl.totalTripDistance; + } else { + // Minimum distance increment between course change track points when GPS course valid + const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_TRIP_DIST_TO_SAVE); + + // Altitude change + if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > NAV_RTH_TRACKBACK_MIN_Z_DIST_TO_SAVE) { + saveTrackpoint = true; + } else if (distanceIncrement && GPSCourseIsValid) { + // Course change - set to 45 degrees + if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) { + saveTrackpoint = true; + } else if (distanceCounter >= 9) { + // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change + // and deviation from projected course path > 20m + float distToPrevPoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]); + + fpVector3_t virtualCoursePoint; + virtualCoursePoint.x = rth_trackback.pointsList[rth_trackback.activePointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse)); + virtualCoursePoint.y = rth_trackback.pointsList[rth_trackback.activePointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse)); + + saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE); + } + distanceCounter++; + previousTBTripDist = posControl.totalTripDistance; + } else if (!GPSCourseIsValid) { + // If no reliable course revert to basic distance logging based on direct distance from last point + saveTrackpoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]) > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE); + previousTBTripDist = posControl.totalTripDistance; + } + + // Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter. + if (distanceCounter && fwLoiterIsActive) { + saveTrackpoint = suspendTracking = true; + } + } + + // When trackpoint store full, overwrite from start of store using 'WrapAroundCounter' to track overwrite position + if (saveTrackpoint) { + if (rth_trackback.activePointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) { // Wraparound to start + rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = 0; + } else { + rth_trackback.activePointIndex++; + if (rth_trackback.WrapAroundCounter > -1) { // Track wraparound overwrite position after store first filled + rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex; + } + } + + rth_trackback.pointsList[rth_trackback.activePointIndex] = posControl.actualState.abs.pos; + rth_trackback.lastSavedIndex = rth_trackback.activePointIndex; + previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z); + previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse; + distanceCounter = 0; + } + } +} + +bool rthTrackBackSetNewPosition(void) +{ + if (posControl.flags.estPosStatus == EST_NONE) { + return false; + } + + const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex])); + +#ifdef USE_MULTI_FUNCTIONS + const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK); +#else + const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL); +#endif + const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance || (overrideTrackback && !posControl.flags.forcedRTHActivated); + + if (rth_trackback.activePointIndex < 0 || cancelTrackback) { + rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1; + posControl.flags.rthTrackbackActive = false; + return true; // Procede to home after final trackback point + } + + if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { + rth_trackback.activePointIndex--; + + if (rth_trackback.WrapAroundCounter > -1 && rth_trackback.activePointIndex < 0) { + rth_trackback.activePointIndex = NAV_RTH_TRACKBACK_POINTS - 1; + } + + calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); + + if (rth_trackback.activePointIndex - rth_trackback.WrapAroundCounter == 0) { + rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1; + } + } else { + setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + + return false; +} + +fpVector3_t *getRthTrackBackPosition(void) +{ + // Ensure trackback altitude never lower than altitude of start point + if (rth_trackback.pointsList[rth_trackback.activePointIndex].z < rth_trackback.pointsList[rth_trackback.lastSavedIndex].z) { + rth_trackback.pointsList[rth_trackback.activePointIndex].z = rth_trackback.pointsList[rth_trackback.lastSavedIndex].z; + } + + return &rth_trackback.pointsList[rth_trackback.activePointIndex]; +} + +void resetRthTrackBack(void) +{ + rth_trackback.activePointIndex = -1; + posControl.flags.rthTrackbackActive = false; + rth_trackback.WrapAroundCounter = -1; +} \ No newline at end of file diff --git a/src/main/navigation/rth_trackback.h b/src/main/navigation/rth_trackback.h new file mode 100644 index 00000000000..2b17803ab99 --- /dev/null +++ b/src/main/navigation/rth_trackback.h @@ -0,0 +1,42 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#include "common/vector.h" + +#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points +#define NAV_RTH_TRACKBACK_MIN_DIST_TO_START 50 // start recording when some distance from home (meters) +#define NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE 20 // minimum XY distance between two points to store in the buffer (meters) +#define NAV_RTH_TRACKBACK_MIN_Z_DIST_TO_SAVE 10 // minimum Z distance between two points to store in the buffer (meters) +#define NAV_RTH_TRACKBACK_MIN_TRIP_DIST_TO_SAVE 10 // minimum trip distance between two points to store in the buffer (meters) + +typedef struct +{ + fpVector3_t pointsList[NAV_RTH_TRACKBACK_POINTS]; // buffer of points stored + int8_t lastSavedIndex; // last trackback point index saved + int8_t activePointIndex; // trackback points counter + int8_t WrapAroundCounter; // stores trackpoint array overwrite index position +} rth_trackback_t; + +extern rth_trackback_t rth_trackback; + +bool rthTrackBackIsActive(void); +bool rthTrackBackSetNewPosition(void); +void rthTrackBackUpdate(bool forceSaveTrackPoint); +fpVector3_t *getRthTrackBackPosition(void); +void resetRthTrackBack(void); \ No newline at end of file From bee51283c998be20a7d40163b632fca9140f398e Mon Sep 17 00:00:00 2001 From: Julio Cesar Date: Mon, 30 Oct 2023 20:50:45 -0300 Subject: [PATCH 114/199] pass some variations from int8_t to int16_t, so that in the future the number of trackback points can be increased --- src/main/navigation/rth_trackback.c | 8 +++----- src/main/navigation/rth_trackback.h | 6 +++--- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/navigation/rth_trackback.c b/src/main/navigation/rth_trackback.c index a5bbd08d83c..37467d9b385 100644 --- a/src/main/navigation/rth_trackback.c +++ b/src/main/navigation/rth_trackback.c @@ -46,7 +46,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint) { static bool suspendTracking = false; bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE)); - + if (!fwLoiterIsActive && suspendTracking) { suspendTracking = false; } @@ -55,8 +55,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint) return; } - // Record trackback points based on significant change in course/altitude until - // points limit reached. Overwrite older points from then on. + // Record trackback points based on significant change in course/altitude until points limit reached. Overwrite older points from then on. if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) { static int32_t previousTBTripDist; // cm static int16_t previousTBCourse; // degrees @@ -82,8 +81,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint) if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) { saveTrackpoint = true; } else if (distanceCounter >= 9) { - // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change - // and deviation from projected course path > 20m + // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change and deviation from projected course path > 20m float distToPrevPoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]); fpVector3_t virtualCoursePoint; diff --git a/src/main/navigation/rth_trackback.h b/src/main/navigation/rth_trackback.h index 2b17803ab99..e3c71d0d45e 100644 --- a/src/main/navigation/rth_trackback.h +++ b/src/main/navigation/rth_trackback.h @@ -28,9 +28,9 @@ typedef struct { fpVector3_t pointsList[NAV_RTH_TRACKBACK_POINTS]; // buffer of points stored - int8_t lastSavedIndex; // last trackback point index saved - int8_t activePointIndex; // trackback points counter - int8_t WrapAroundCounter; // stores trackpoint array overwrite index position + int16_t lastSavedIndex; // last trackback point index saved + int16_t activePointIndex; // trackback points counter + int16_t WrapAroundCounter; // stores trackpoint array overwrite index position } rth_trackback_t; extern rth_trackback_t rth_trackback; From c2577e477733ceb602859a9f35baa24925cc9365 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 31 Oct 2023 10:34:49 +0100 Subject: [PATCH 115/199] Put getConfigMixerProfile as the last byte in message --- src/main/fc/fc_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f504ffb0598..fddcde00ebf 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -462,9 +462,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, packSensorStatus()); sbufWriteU16(dst, averageSystemLoadPercent); sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile()); - sbufWriteU8(dst, getConfigMixerProfile()); sbufWriteU32(dst, armingFlags); sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags)); + sbufWriteU8(dst, getConfigMixerProfile()); } break; From fac64cecbbe768bfdaab09046d6b13e2bf0dd297 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 31 Oct 2023 21:41:16 +0000 Subject: [PATCH 116/199] Update navigation.c --- src/main/navigation/navigation.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cb12579b83d..c991531f32b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4055,7 +4055,8 @@ bool navigationPositionEstimateIsHealthy(void) navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { - const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE))); + const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || + IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)); if (usedBypass) { *usedBypass = false; From 298f45e7f78222434fbf2833b1756fea07bf48dc Mon Sep 17 00:00:00 2001 From: MrD-RC Date: Thu, 2 Nov 2023 12:50:12 +0000 Subject: [PATCH 117/199] Move parameter out of battery profile, in to normal settings. --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 12 ++++++------ src/main/navigation/navigation.c | 3 ++- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fixedwing.c | 6 +++--- src/main/sensors/battery.c | 4 +--- src/main/sensors/battery_config_structs.h | 2 -- 7 files changed, 14 insertions(+), 16 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 0bf30136291..f111290c142 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3698,7 +3698,7 @@ Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s. | Default | Min | Max | | --- | --- | --- | -| 7 | 6 | 45 | +| 7 | 6 | 50 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bf007b96b3b..0cdac1ac93f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1046,12 +1046,6 @@ groups: default_value: 1000 min: PWM_RANGE_MIN max: PWM_RANGE_MAX - - name: nav_min_ground_speed - description: "Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s." - default_value: 7 - field: nav.min_ground_speed - min: 6 - max: 45 - name: nav_mc_hover_thr description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering." default_value: 1300 @@ -2492,6 +2486,12 @@ groups: field: general.auto_speed min: 10 max: 2000 + - name: nav_min_ground_speed + description: "Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s." + default_value: 7 + field: general.min_ground_speed + min: 6 + max: 50 - name: nav_max_auto_speed description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]" default_value: 1000 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cb12579b83d..800e51371e8 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -97,7 +97,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -132,6 +132,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, #endif .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h) + .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s) .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ecc487cc9e7..adce68539c0 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -245,6 +245,7 @@ typedef struct navConfig_s { #endif bool waypoint_load_on_boot; // load waypoints automatically during boot uint16_t auto_speed; // autonomous navigation speed cm/sec + uint8_t min_ground_speed; // Minimum navigation ground speed [m/s] uint16_t max_auto_speed; // maximum allowed autonomous navigation speed cm/sec uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec uint16_t max_manual_speed; // manual velocity control max horizontal speed diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 148617a9a49..3c60f2f43d3 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -60,7 +60,7 @@ #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f -// If we are going slower than the minimum ground speed (currentBatteryProfile->nav.min_ground_speed) - boost throttle to fight against the wind +// If we are going slower than the minimum ground speed (navConfig()->general.min_ground_speed) - boost throttle to fight against the wind #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN 1.5f // If this is enabled navigation won't be applied if velocity is below 3 m/s @@ -551,10 +551,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) previousTimePositionUpdate = currentTimeUs; if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - float velThrottleBoost = ((currentBatteryProfile->nav.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); + float velThrottleBoost = ((navConfig()->general.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); // If we are in the deadband of 50cm/s - don't update speed boost - if (fabsf(posControl.actualState.velXY - (currentBatteryProfile->nav.min_ground_speed * 100.0f)) > 50) { + if (fabsf(posControl.actualState.velXY - (navConfig()->general.min_ground_speed * 100.0f)) > 50) { throttleSpeedAdjustment += velThrottleBoost; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 3273c2d0578..3745ed7a8ea 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -97,7 +97,7 @@ static int32_t mWhDrawn = 0; // energy (milliWatt hours) draw batteryState_e batteryState; const batteryProfile_t *currentBatteryProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 3); +PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 2); void pgResetFn_batteryProfiles(batteryProfile_t *instance) { @@ -134,8 +134,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off. .nav = { - .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, - .mc = { .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT, }, diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h index 183a1fd2703..81eafef44ad 100644 --- a/src/main/sensors/battery_config_structs.h +++ b/src/main/sensors/battery_config_structs.h @@ -123,8 +123,6 @@ typedef struct batteryProfile_s { uint16_t launch_throttle; // Launch throttle } fw; - uint8_t min_ground_speed; // Minimum navigation ground speed [m/s] - } nav; #if defined(USE_POWER_LIMITS) From 3a9624ba492c84afb7ff103466aefa017849fc53 Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 3 Nov 2023 21:30:41 +0900 Subject: [PATCH 118/199] sync updateActualHeading with other topic --- .../navigation/navigation_pos_estimator.c | 22 ++++++++----------- .../navigation_pos_estimator_private.h | 1 - 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 1b561e98810..8157fa730cd 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -707,15 +707,10 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); if (STATE(GPS_FIX) && navIsHeadingUsable()) { - static timeUs_t lastUpdateTimeUs = 0; - - if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate - const float dt = US2S(currentTimeUs - lastUpdateTimeUs); - uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt))); - posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse); - lastUpdateTimeUs = currentTimeUs; - } + uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y, posEstimator.est.vel.x))); + posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse); } } @@ -813,13 +808,14 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) { static navigationTimer_t posPublishTimer; - /* IMU operates in decidegrees while INAV operates in deg*100 - * Use course over ground when GPS heading valid */ - int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw; - updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue)); - /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */ if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) { + /* Publish heading update */ + /* IMU operates in decidegrees while INAV operates in deg*100 + * Use course over ground when GPS heading valid */ + int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw; + updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue)); + /* Publish position update */ if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) { // FIXME!!!!! diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 40129f844b0..0b19dbb0023 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -39,7 +39,6 @@ #define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate #define INAV_PITOT_UPDATE_RATE 10 -#define INAV_COG_UPDATE_RATE_HZ 20 // ground course update rate #define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout #define INAV_BARO_TIMEOUT_MS 200 // Baro timeout From f3359efbc6010b29cc7b3ef6cd256811f5064962 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 3 Nov 2023 15:46:19 +0200 Subject: [PATCH 119/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index ed67682353f..28e2f170f68 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -19,7 +19,7 @@ By befault, all navigation modes are disabled when GPS fix is lost. If RC signal GPS fix estimation allows to recover plane using magnetometer and baromener only. -Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated. GPS Fix is also estimated on GPS Sensor timeouts (hardware failures). From 229eac45f08c3cf9fd15d124b3f2979046e1d697 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 3 Nov 2023 15:47:12 +0200 Subject: [PATCH 120/199] Update GPS_fix_estimation.md --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index 28e2f170f68..d8175549d35 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -19,10 +19,10 @@ By befault, all navigation modes are disabled when GPS fix is lost. If RC signal GPS fix estimation allows to recover plane using magnetometer and baromener only. -Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated. - GPS Fix is also estimated on GPS Sensor timeouts (hardware failures). +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated. + # How it works ? In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints. From 6f1ad0a9f3302e01046f8c93eddf3227f5fff8ed Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 4 Nov 2023 02:01:29 +0900 Subject: [PATCH 121/199] add more gps dynamic model --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 6 +++--- src/main/io/gps.h | 2 ++ src/main/io/gps_ublox.c | 10 ++++++++-- src/main/io/gps_ublox.h | 2 ++ 5 files changed, 17 insertions(+), 7 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index cb760e312ab..1f0fe29c419 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1484,11 +1484,11 @@ Enable automatic configuration of UBlox GPS receivers. ### gps_dyn_model -GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. +GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying. | Default | Min | Max | | --- | --- | --- | -| AIR_1G | | | +| AIR_2G | | | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9fb1c051a94..eed0f699744 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -50,7 +50,7 @@ tables: values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"] enum: sbasMode_e - name: gps_dyn_model - values: ["PEDESTRIAN", "AIR_1G", "AIR_4G"] + values: ["PEDESTRIAN","AUTOMOTIVE", "AIR_1G", "AIR_2G", "AIR_4G"] enum: gpsDynModel_e - name: reset_type values: ["NEVER", "FIRST_ARM", "EACH_ARM"] @@ -1614,8 +1614,8 @@ groups: table: gps_sbas_mode type: uint8_t - name: gps_dyn_model - description: "GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying." - default_value: "AIR_1G" + description: "GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying." + default_value: "AIR_2G" field: dynModel table: gps_dyn_model type: uint8_t diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 199652a3a24..3e9d073c54c 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -76,7 +76,9 @@ typedef enum { typedef enum { GPS_DYNMODEL_PEDESTRIAN = 0, + GPS_DYNMODEL_AUTOMOTIVE, GPS_DYNMODEL_AIR_1G, + GPS_DYNMODEL_AIR_2G, GPS_DYNMODEL_AIR_4G, } gpsDynModel_e; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index d03c3390776..a9455e003ba 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -800,10 +800,16 @@ STATIC_PROTOTHREAD(gpsConfigure) case GPS_DYNMODEL_PEDESTRIAN: configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO); break; - case GPS_DYNMODEL_AIR_1G: // Default to this - default: + case GPS_DYNMODEL_AUTOMOTIVE: + configureNAV5(UBX_DYNMODEL_AUTOMOVITE, UBX_FIXMODE_AUTO); + break; + case GPS_DYNMODEL_AIR_1G: configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO); break; + case GPS_DYNMODEL_AIR_2G: // Default to this + default: + configureNAV5(UBX_DYNMODEL_AIR_2G, UBX_FIXMODE_AUTO); + break; case GPS_DYNMODEL_AIR_4G: configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO); break; diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 00b42eeb2b7..8ec9be16d09 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -34,7 +34,9 @@ extern "C" { #define GPS_CAPA_INTERVAL 5000 #define UBX_DYNMODEL_PEDESTRIAN 3 +#define UBX_DYNMODEL_AUTOMOVITE 4 #define UBX_DYNMODEL_AIR_1G 6 +#define UBX_DYNMODEL_AIR_2G 7 #define UBX_DYNMODEL_AIR_4G 8 #define UBX_FIXMODE_2D_ONLY 1 From 52579eb84bb45e21d5b38aef50fa1cabaa0acd5c Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sun, 5 Nov 2023 09:40:11 +0000 Subject: [PATCH 122/199] [GPS] fix GPS discovery for M6 and earlier --- src/main/io/gps_ublox.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index d03c3390776..8fa8366a23c 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1042,12 +1042,14 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) gpsState.autoConfigStep = 0; ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; - do { - pollGnssCapabilities(); - gpsState.autoConfigStep++; - ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); - } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); - + // M6 and earlier will never get pass this step, so skip it (#9440) + if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX6) { + do { + pollGnssCapabilities(); + gpsState.autoConfigStep++; + ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); + } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); + } // Configure GPS module if enabled if (gpsState.gpsConfig->autoConfig) { // Configure GPS From d135f3ece75de508035a7bf2edb1661457534a3b Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sun, 5 Nov 2023 10:23:47 +0000 Subject: [PATCH 123/199] [GPS] update discovery fix for M7N as well --- src/main/io/gps_ublox.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 8fa8366a23c..da0f54dd2fb 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1042,8 +1042,9 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) gpsState.autoConfigStep = 0; ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; - // M6 and earlier will never get pass this step, so skip it (#9440) - if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX6) { + // M7 and earlier will never get pass this step, so skip it (#9440). + // UBLOX documents that this is M8N and later + if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) { do { pollGnssCapabilities(); gpsState.autoConfigStep++; From 4f3b888dc56b0578560e53dfff404f4e7e7869eb Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 5 Nov 2023 11:42:51 +0100 Subject: [PATCH 124/199] ATOMRCF405NAVI_DELUX first take --- .../ATOMRCF405NAVI_DELUX/CMakeLists.txt | 1 + src/main/target/ATOMRCF405NAVI_DELUX/config.c | 37 ++++ src/main/target/ATOMRCF405NAVI_DELUX/target.c | 56 ++++++ src/main/target/ATOMRCF405NAVI_DELUX/target.h | 189 ++++++++++++++++++ 4 files changed, 283 insertions(+) create mode 100644 src/main/target/ATOMRCF405NAVI_DELUX/CMakeLists.txt create mode 100644 src/main/target/ATOMRCF405NAVI_DELUX/config.c create mode 100644 src/main/target/ATOMRCF405NAVI_DELUX/target.c create mode 100644 src/main/target/ATOMRCF405NAVI_DELUX/target.h diff --git a/src/main/target/ATOMRCF405NAVI_DELUX/CMakeLists.txt b/src/main/target/ATOMRCF405NAVI_DELUX/CMakeLists.txt new file mode 100644 index 00000000000..b9b7af86341 --- /dev/null +++ b/src/main/target/ATOMRCF405NAVI_DELUX/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(ATOMRCF405NAVI_DELUX) diff --git a/src/main/target/ATOMRCF405NAVI_DELUX/config.c b/src/main/target/ATOMRCF405NAVI_DELUX/config.c new file mode 100644 index 00000000000..e8d9b303920 --- /dev/null +++ b/src/main/target/ATOMRCF405NAVI_DELUX/config.c @@ -0,0 +1,37 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200; + + pinioBoxConfigMutable()->permanentId[0] = BOXARM; +} diff --git a/src/main/target/ATOMRCF405NAVI_DELUX/target.c b/src/main/target/ATOMRCF405NAVI_DELUX/target.c new file mode 100644 index 00000000000..e3ff9d8f77e --- /dev/null +++ b/src/main/target/ATOMRCF405NAVI_DELUX/target.c @@ -0,0 +1,56 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM8, CH2N, PB14, TIM_USE_SERVO, 0, 0), // S10 + + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_SERVO, 0, 0), // S11 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_STRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ATOMRCF405NAVI_DELUX/target.h b/src/main/target/ATOMRCF405NAVI_DELUX/target.h new file mode 100644 index 00000000000..1134daa29f1 --- /dev/null +++ b/src/main/target/ATOMRCF405NAVI_DELUX/target.h @@ -0,0 +1,189 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once +#define TARGET_BOARD_IDENTIFIER "AT4D" + +#define USBD_PRODUCT_STRING "AtomRCF405NAVI_DELUX" + +#define LED0 PA13 +#define LED1 PA14 + +#define BEEPER PB2 +#define BEEPER_INVERTED + +/* + * SPI defines + */ +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +/* + * I2C defines + */ +#define USE_I2C +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +//ICM42688-P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +/* + * Magnetometer + */ +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +/* + * Barometer + */ +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL + +/* + * Serial ports + */ +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +/* + * ADC + */ +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC4 +#define ADC_CHANNEL_4_PIN PC5 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +#define CURRENT_METER_SCALE 320 + +/* + * OSD + */ +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +/* + * SD Card + */ +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC14 +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +/* + * LED Strip + */ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +/* + * Other configs + */ +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT ) +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define MAX_PWM_OUTPUT_PORTS 11 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC15 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED \ No newline at end of file From 50930eee9a9693c51fe0d03ae00dcc447c260e05 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 6 Nov 2023 10:42:53 +0100 Subject: [PATCH 125/199] Add DPS310 and SPL06 to FLYWOOF745 target This was requested by flywoo support. Also add other baros to match their internal target. --- src/main/target/FLYWOOF745/target.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/target/FLYWOOF745/target.h b/src/main/target/FLYWOOF745/target.h index 3b57ccc4fa2..c2b1937c611 100644 --- a/src/main/target/FLYWOOF745/target.h +++ b/src/main/target/FLYWOOF745/target.h @@ -140,6 +140,10 @@ #define USE_BARO #define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 #define BARO_I2C_BUS BUS_I2C1 #define USE_MAG From 9783a7b176c9ea40d6c2ebd4fce9f84a9490a035 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 6 Nov 2023 21:21:31 +0100 Subject: [PATCH 126/199] Remove PPM entry with conflicting timer. Fixes #9442 --- src/main/target/KAKUTEF7/target.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c index 53ae9359deb..ce7c14e9474 100755 --- a/src/main/target/KAKUTEF7/target.c +++ b/src/main/target/KAKUTEF7/target.c @@ -29,7 +29,6 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 @@ -37,8 +36,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 + // + //DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 1), // PPM/M7, DMA2_ST6 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); From 1f597089d4935aa5af65b0006120f675b215c94e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 6 Nov 2023 21:25:40 +0100 Subject: [PATCH 127/199] cleanup --- src/main/target/KAKUTEF7/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) mode change 100755 => 100644 src/main/target/KAKUTEF7/target.c diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c old mode 100755 new mode 100644 index ce7c14e9474..19d524dc710 --- a/src/main/target/KAKUTEF7/target.c +++ b/src/main/target/KAKUTEF7/target.c @@ -36,7 +36,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 - // + //DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 1), // PPM/M7, DMA2_ST6 DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 From 3ef172dacdde07a73a4ab25c504c1c007b572b6b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 6 Nov 2023 21:55:00 +0100 Subject: [PATCH 128/199] Various PWM output changes. Mainly to get rid of potential conflicts with new timer allocation --- .vscode/settings.json | 3 ++- src/main/target/AIRBOTF4/target.c | 13 ++++----- src/main/target/ALIENFLIGHTF4/target.c | 10 +++---- src/main/target/ALIENFLIGHTNGF7/target.c | 2 +- src/main/target/ANYFC/target.c | 12 ++++----- src/main/target/ANYFCF7/target.c | 13 ++++----- src/main/target/ANYFCM7/target.c | 12 ++++----- src/main/target/AOCODARCF405AIO/target.c | 2 +- src/main/target/AOCODARCF4V2/target.c | 2 +- src/main/target/AOCODARCF722AIO/target.c | 2 +- src/main/target/AOCODARCF7DUAL/target.c | 2 +- src/main/target/AOCODARCF7MINI/target.c | 2 +- src/main/target/AXISFLYINGF7PRO/target.c | 2 +- src/main/target/BEEROTORF4/target.c | 2 +- src/main/target/BETAFLIGHTF4/target.c | 2 +- src/main/target/BETAFPVF722/target.c | 2 +- src/main/target/BLACKPILL_F411/target.c | 2 +- src/main/target/BLUEJAYF4/target.c | 2 +- src/main/target/CLRACINGF4AIR/target.c | 4 +-- src/main/target/COLIBRI/target.c | 16 +++++------ src/main/target/DALRCF405/target.c | 2 +- src/main/target/DALRCF722DUAL/target.c | 2 +- src/main/target/F4BY/target.c | 16 +++++------ src/main/target/FF_F35_LIGHTNING/target.c | 10 +++---- src/main/target/FF_FORTINIF4/target.c | 2 +- src/main/target/FIREWORKSV2/target.c | 2 +- src/main/target/FISHDRONEF4/target.c | 12 ++++----- src/main/target/FLYWOOF405PRO/target.c | 2 +- src/main/target/FLYWOOF411/target.c | 2 +- src/main/target/FLYWOOF745/target.c | 2 +- src/main/target/FLYWOOF7DUAL/target.c | 2 +- src/main/target/FOXEERF405/target.c | 2 +- src/main/target/FOXEERF722DUAL/target.c | 2 +- src/main/target/FURYF4OSD/target.c | 2 +- src/main/target/GEPRCF405/target.c | 6 +---- src/main/target/GEPRCF722/target.c | 2 +- src/main/target/GEPRCF722_BT_HD/target.c | 4 +-- src/main/target/GEPRC_F722_AIO/target.c | 2 +- src/main/target/HAKRCF405D/target.c | 8 +++--- src/main/target/HAKRCF411D/target.c | 2 +- src/main/target/HAKRCKD722/target.c | 2 +- src/main/target/HGLRCF722/target.c | 4 +-- src/main/target/IFLIGHTF4_SUCCEXD/target.c | 2 +- src/main/target/IFLIGHTF4_TWING/target.c | 2 +- src/main/target/IFLIGHTF7_TWING/target.c | 2 +- src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c | 2 +- src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c | 2 +- src/main/target/IFLIGHT_JBF7PRO/target.c | 2 +- src/main/target/KAKUTEF4/target.c | 2 +- src/main/target/KAKUTEF7/target.c | 2 +- src/main/target/KAKUTEF7MINIV3/target.c | 2 +- src/main/target/KROOZX/target.c | 2 +- src/main/target/MAMBAF405US/target.c | 2 +- src/main/target/MAMBAF722/target.c | 2 +- src/main/target/MAMBAF722_2022A/target.c | 2 +- src/main/target/MAMBAF722_X8/target.c | 2 +- src/main/target/MATEKF405/target.c | 2 +- src/main/target/MATEKF405CAN/target.c | 2 +- src/main/target/MATEKF405SE/target.c | 2 +- src/main/target/MATEKF405TE/target.c | 2 +- src/main/target/MATEKF411/target.c | 2 +- src/main/target/MATEKF722/target.c | 2 +- src/main/target/MATEKF722PX/target.c | 7 +++-- src/main/target/MATEKF722SE/target.c | 4 +-- src/main/target/MATEKF765/target.c | 4 +-- src/main/target/MATEKH743/target.c | 8 +++--- src/main/target/NEUTRONRCF435SE/target.c | 27 +++++++++---------- src/main/target/NOX/target.c | 5 ++-- src/main/target/OMNIBUSF4/target.c | 24 +++++++++-------- src/main/target/OMNIBUSF7NXT/target.c | 2 +- src/main/target/PIXRACER/target.c | 2 +- src/main/target/RADIX/target.c | 2 +- src/main/target/REVO/target.c | 12 ++++----- src/main/target/SAGEATF4/target.c | 6 ++--- src/main/target/SKYSTARSF405HD/target.c | 2 +- src/main/target/SKYSTARSF722HD/target.c | 2 +- src/main/target/SPARKY2/target.c | 10 +++---- src/main/target/SPEEDYBEEF4/target.c | 2 +- src/main/target/SPEEDYBEEF7/target.c | 2 +- src/main/target/SPEEDYBEEF7MINI/target.c | 2 +- src/main/target/SPEEDYBEEF7V2/target.c | 2 +- src/main/target/SPRACINGF4EVO/target.c | 4 +-- src/main/target/SPRACINGF7DUAL/target.c | 4 +-- src/main/target/TMOTORF7/target.c | 2 +- src/main/target/TMOTORF7V2/target.c | 2 +- src/main/target/YUPIF4/target.c | 2 +- src/main/target/YUPIF7/target.c | 2 +- 87 files changed, 188 insertions(+), 190 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 2cece3ee127..f95df72f1fa 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,7 +2,8 @@ "files.associations": { "chrono": "c", "cmath": "c", - "ranges": "c" + "ranges": "c", + "platform.h": "c" }, "editor.tabSize": 4, "editor.insertSpaces": true, diff --git a/src/main/target/AIRBOTF4/target.c b/src/main/target/AIRBOTF4/target.c index 988543aa321..9273b3dff7f 100644 --- a/src/main/target/AIRBOTF4/target.c +++ b/src/main/target/AIRBOTF4/target.c @@ -32,12 +32,13 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED | TIM_USE_ANY, 0, 0), // LED D1_ST0, n/a on older AIRBOTF4 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S3_IN - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S4_IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S5_IN - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S6_IN + //Airbot F4 don't have those outputs, they exist only in the original Revo + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN + // DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S3_IN + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S4_IN + // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S5_IN + // DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S6_IN }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index e7ee4cf5520..7c6fd6ed84a 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -23,11 +23,10 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 1), // PWM1 - PA8 RC1 - DMA2_ST1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2 - DMA1_ST5 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3 - DMA1_ST7 - DEF_TIM(TIM1, CH2, PB14, TIM_USE_PWM, 0, 1), // PWM4 - PA14 RC4 - DMA2_ST2 - DEF_TIM(TIM1, CH3, PB15, TIM_USE_PWM | TIM_USE_LED, 0, 0), // PWM5 - PA15 RC5 - DMA2_ST6 + // DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 1), // PWM1 - PA8 RC1 - DMA2_ST1 + // DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2 - DMA1_ST5 + // DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3 - DMA1_ST7 + // DEF_TIM(TIM1, CH2, PB14, TIM_USE_PWM, 0, 1), // PWM4 - PA14 RC4 - DMA2_ST2 DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM6 - PB8 OUT1 - DMA1_ST7 DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM7 - PB9 OUT2 - DMA_NONE DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM8 - PA0 OUT3 - DMA1_ST2 @@ -36,6 +35,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM11 - PC7 OUT6 - DMA2_ST3 DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // PWM13 - PC8 OUT7 - DMA2_ST4 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM13 - PC9 OUT8 - DMA2_ST7 + DEF_TIM(TIM1, CH3, PB15, TIM_USE_LED, 0, 0), // PWM5 - PA15 RC5 - DMA2_ST6 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ALIENFLIGHTNGF7/target.c b/src/main/target/ALIENFLIGHTNGF7/target.c index 455747923ef..ba8ab6111c3 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.c +++ b/src/main/target/ALIENFLIGHTNGF7/target.c @@ -23,7 +23,6 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM | TIM_USE_LED, 0, 1), // PPM - DMA2_ST1 DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM1 - DMA2_ST2 DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM2 - DMA1_ST5 DEF_TIM(TIM8, CH2N, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM3 - DMA2_ST3 @@ -36,6 +35,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM10 - DMA2_ST6 DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM11 - DMA1_ST7 DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // PWM12 - DMA_NONE + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1), // PPM - DMA2_ST1 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ANYFC/target.c b/src/main/target/ANYFC/target.c index 5353b589110..a8adb8578e3 100644 --- a/src/main/target/ANYFC/target.c +++ b/src/main/target/ANYFC/target.c @@ -23,12 +23,12 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // S6_IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S5_IN + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN + // DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN + // DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0), // S6_IN + // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S5_IN DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S10_OUT 16 DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT 12 diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index 65f807d7984..dc523f4c780 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -23,12 +23,8 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_IN DMA2_ST7 + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7 DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST0 @@ -40,6 +36,11 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7_OUT DMA1_ST2 DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S8_OUT DMA1_ST6 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S9_OUT DMA1_ST4 + + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_IN DMA2_ST7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ANYFCM7/target.c b/src/main/target/ANYFCM7/target.c index fb69d60a43b..7dd30d2250b 100644 --- a/src/main/target/ANYFCM7/target.c +++ b/src/main/target/ANYFCM7/target.c @@ -26,12 +26,12 @@ #define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL timerHardware_t timerHardware[] = { - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN + // DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 + // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + // DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7 DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST1 diff --git a/src/main/target/AOCODARCF405AIO/target.c b/src/main/target/AOCODARCF405AIO/target.c index be225da1bf6..d2f3d67c427 100644 --- a/src/main/target/AOCODARCF405AIO/target.c +++ b/src/main/target/AOCODARCF405AIO/target.c @@ -28,7 +28,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU65 BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH2, PA10, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM2, CH2, PA10, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 diff --git a/src/main/target/AOCODARCF4V2/target.c b/src/main/target/AOCODARCF4V2/target.c index 8581cc5bf11..eccd2f06d8c 100644 --- a/src/main/target/AOCODARCF4V2/target.c +++ b/src/main/target/AOCODARCF4V2/target.c @@ -25,7 +25,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1 DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S2 diff --git a/src/main/target/AOCODARCF722AIO/target.c b/src/main/target/AOCODARCF722AIO/target.c index 90d21bbf2f6..038b4121b48 100644 --- a/src/main/target/AOCODARCF722AIO/target.c +++ b/src/main/target/AOCODARCF722AIO/target.c @@ -27,7 +27,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S1 DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // S2 diff --git a/src/main/target/AOCODARCF7DUAL/target.c b/src/main/target/AOCODARCF7DUAL/target.c index d09fafeda21..2e7de30b5d8 100644 --- a/src/main/target/AOCODARCF7DUAL/target.c +++ b/src/main/target/AOCODARCF7DUAL/target.c @@ -30,7 +30,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), //PPM + // DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), //PPM DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(2, 2, 7) DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2, 3, 7) diff --git a/src/main/target/AOCODARCF7MINI/target.c b/src/main/target/AOCODARCF7MINI/target.c index 34360bbc033..f01d2c73e81 100644 --- a/src/main/target/AOCODARCF7MINI/target.c +++ b/src/main/target/AOCODARCF7MINI/target.c @@ -37,7 +37,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1 + // DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1, 4, 5) DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1, 5, 4) diff --git a/src/main/target/AXISFLYINGF7PRO/target.c b/src/main/target/AXISFLYINGF7PRO/target.c index 8d4b966b69c..e6ad7b071f2 100644 --- a/src/main/target/AXISFLYINGF7PRO/target.c +++ b/src/main/target/AXISFLYINGF7PRO/target.c @@ -33,7 +33,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // CAM CONTROL }; diff --git a/src/main/target/BEEROTORF4/target.c b/src/main/target/BEEROTORF4/target.c index 28e6a77402a..aa4d4893bf1 100644 --- a/src/main/target/BEEROTORF4/target.c +++ b/src/main/target/BEEROTORF4/target.c @@ -24,7 +24,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 - DMAR: DMA2_ST5 DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 - diff --git a/src/main/target/BETAFLIGHTF4/target.c b/src/main/target/BETAFLIGHTF4/target.c index 0e10cdbf59d..67d8f260adc 100755 --- a/src/main/target/BETAFLIGHTF4/target.c +++ b/src/main/target/BETAFLIGHTF4/target.c @@ -24,7 +24,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM // Motors DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 diff --git a/src/main/target/BETAFPVF722/target.c b/src/main/target/BETAFPVF722/target.c index e24689c0ca9..d44324cd7ce 100755 --- a/src/main/target/BETAFPVF722/target.c +++ b/src/main/target/BETAFPVF722/target.c @@ -25,7 +25,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1 D(1, 4, 5) DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2 D(2, 3, 7) diff --git a/src/main/target/BLACKPILL_F411/target.c b/src/main/target/BLACKPILL_F411/target.c index d36b8c19c24..18c0d2edf6a 100644 --- a/src/main/target/BLACKPILL_F411/target.c +++ b/src/main/target/BLACKPILL_F411/target.c @@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), //ST1 pad -softserial_tx1 - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM }; diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index 0b968380cf0..ba48551f76c 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -23,7 +23,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT - DMA1_ST2 DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - DMA1_ST4 diff --git a/src/main/target/CLRACINGF4AIR/target.c b/src/main/target/CLRACINGF4AIR/target.c index 54fa14c7097..beca9ba67ec 100644 --- a/src/main/target/CLRACINGF4AIR/target.c +++ b/src/main/target/CLRACINGF4AIR/target.c @@ -21,8 +21,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), + // DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0), #if defined(CLRACINGF4AIRV2) || defined(CLRACINGF4AIRV3) DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), @@ -39,6 +38,7 @@ DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), #endif + DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index 932188992bc..7affcc4936c 100755 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -24,14 +24,14 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PA10, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S2_IN - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S3_IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S4_IN - DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, 0, 0), // S5_IN - DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM, 0, 0), // S6_IN - DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // S7_IN - DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // S8_IN + // DEF_TIM(TIM1, CH3, PA10, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // S1_IN + // DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0), // S2_IN + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0), // S3_IN + // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0), // S4_IN + // DEF_TIM(TIM2, CH1, PA15, TIM_USE_PWM, 0, 0), // S5_IN + // DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM, 0, 0), // S6_IN + // DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // S7_IN + // DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // S8_IN DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT diff --git a/src/main/target/DALRCF405/target.c b/src/main/target/DALRCF405/target.c index e1cddea2613..816ad3cdeca 100644 --- a/src/main/target/DALRCF405/target.c +++ b/src/main/target/DALRCF405/target.c @@ -22,7 +22,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (1,7) DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (2,2) diff --git a/src/main/target/DALRCF722DUAL/target.c b/src/main/target/DALRCF722DUAL/target.c index 286c00539a3..8c4fa74475e 100644 --- a/src/main/target/DALRCF722DUAL/target.c +++ b/src/main/target/DALRCF722DUAL/target.c @@ -28,7 +28,7 @@ #define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM ,0, 0), + // DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM ,0, 0), DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO,0,0), //S1 DMA2_ST2 T8CH1 DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO,0,0), //S2 DMA2_ST3 T8CH2 diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index 37d9db89418..8d1b4622f69 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -6,14 +6,14 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN - DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN - DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN - DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN - DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN - DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN - DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN - DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN + // DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN + // DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN + // DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN + // DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN + // DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN + // DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN + // DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN + // DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT diff --git a/src/main/target/FF_F35_LIGHTNING/target.c b/src/main/target/FF_F35_LIGHTNING/target.c index 83d75497e4d..617c085a925 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.c +++ b/src/main/target/FF_F35_LIGHTNING/target.c @@ -33,11 +33,11 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), DEF_TIM(TIM5, CH4, PA3, TIM_USE_ANY, 0, 0), -#ifdef WINGFC - DEF_TIM(TIM2, CH4, PB11, TIM_USE_PPM, 0, 0), // UART3 RX -#else - DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), // UART3 TX -#endif +// #ifdef WINGFC +// DEF_TIM(TIM2, CH4, PB11, TIM_USE_PPM, 0, 0), // UART3 RX +// #else +// DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), // UART3 TX +// #endif }; diff --git a/src/main/target/FF_FORTINIF4/target.c b/src/main/target/FF_FORTINIF4/target.c index 5853bc31391..54aae5ce4e6 100644 --- a/src/main/target/FF_FORTINIF4/target.c +++ b/src/main/target/FF_FORTINIF4/target.c @@ -27,7 +27,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S2_OUT - DMA1_ST2 DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 1, 1 ), // S3_OUT - DMA1_ST6 DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 1, 0 ), // S4_OUT - DMA1_ST1 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM4, CH4, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0), // LED - DMA1_ST3 }; diff --git a/src/main/target/FIREWORKSV2/target.c b/src/main/target/FIREWORKSV2/target.c index 8e7cdb95754..7f96e097145 100644 --- a/src/main/target/FIREWORKSV2/target.c +++ b/src/main/target/FIREWORKSV2/target.c @@ -44,7 +44,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6000, DEVHW_MPU6000, IMU_2_SPI_BUS, IMU_ BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6500, DEVHW_MPU6500, IMU_2_SPI_BUS, IMU_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_2_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM // Motor output 1: use different set of timers for MC and FW //DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM _USE_SERVO, 1, 0), // S1_OUT D(1,7) diff --git a/src/main/target/FISHDRONEF4/target.c b/src/main/target/FISHDRONEF4/target.c index b0aec49bc4d..93354b51758 100644 --- a/src/main/target/FISHDRONEF4/target.c +++ b/src/main/target/FISHDRONEF4/target.c @@ -23,12 +23,12 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0 ), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0 ), - DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0, 0 ), - DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0, 0 ), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM, 0, 0 ), + // DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0 ), + // DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0, 0 ), + // DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0, 0 ), + // DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0, 0 ), + // DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM, 0, 0 ), DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0 ), DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0 ), diff --git a/src/main/target/FLYWOOF405PRO/target.c b/src/main/target/FLYWOOF405PRO/target.c index f61d06ca0e1..c0f1c4eec2d 100644 --- a/src/main/target/FLYWOOF405PRO/target.c +++ b/src/main/target/FLYWOOF405PRO/target.c @@ -34,7 +34,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) - DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYWOOF411/target.c b/src/main/target/FLYWOOF411/target.c index 965974162b0..3946ad0b404 100644 --- a/src/main/target/FLYWOOF411/target.c +++ b/src/main/target/FLYWOOF411/target.c @@ -25,7 +25,7 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH1, PA2, TIM_USE_PPM, 0, 0), // PPM IN + // DEF_TIM(TIM9, CH1, PA2, TIM_USE_PPM, 0, 0), // PPM IN #ifdef FLYWOOF411_V2 DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 - D(2,1) DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 - D(1,6) diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c index 879b66d01fa..7c79d1f6a52 100644 --- a/src/main/target/FLYWOOF745/target.c +++ b/src/main/target/FLYWOOF745/target.c @@ -30,7 +30,7 @@ #include "drivers/pinio.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 + // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 diff --git a/src/main/target/FLYWOOF7DUAL/target.c b/src/main/target/FLYWOOF7DUAL/target.c index d9d5482e3a5..28d5a2ee471 100644 --- a/src/main/target/FLYWOOF7DUAL/target.c +++ b/src/main/target/FLYWOOF7DUAL/target.c @@ -38,7 +38,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2, DEVHW_MPU6000, GYRO_2_SPI_BUS, GYRO BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500_2, DEVHW_MPU6500, GYRO_2_SPI_BUS, GYRO_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, GYRO_2_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PPM, 0, 0), // PPM&SBUS + // DEF_TIM(TIM8, CH1, PC6, TIM_USE_PPM, 0, 0), // PPM&SBUS DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 - D(1,2) DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 - D(1,4) diff --git a/src/main/target/FOXEERF405/target.c b/src/main/target/FOXEERF405/target.c index 1ad827bb5ca..ca6ebe3507e 100644 --- a/src/main/target/FOXEERF405/target.c +++ b/src/main/target/FOXEERF405/target.c @@ -22,7 +22,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (1,7) DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (1,4) diff --git a/src/main/target/FOXEERF722DUAL/target.c b/src/main/target/FOXEERF722DUAL/target.c index 1933a5134ee..fe34e0a45d4 100644 --- a/src/main/target/FOXEERF722DUAL/target.c +++ b/src/main/target/FOXEERF722DUAL/target.c @@ -29,7 +29,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, #endif timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM&SBUS + // DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM&SBUS DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 - D(2, 1, 6) #if defined(FOXEERF722V2) diff --git a/src/main/target/FURYF4OSD/target.c b/src/main/target/FURYF4OSD/target.c index c820d0c378f..ef414db35e4 100644 --- a/src/main/target/FURYF4OSD/target.c +++ b/src/main/target/FURYF4OSD/target.c @@ -23,7 +23,7 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM8, CH4, PC9, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1_OUT - D(1, 6, 3) DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - D(1, 7, 5) DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - D(1, 2, 5) diff --git a/src/main/target/GEPRCF405/target.c b/src/main/target/GEPRCF405/target.c index 68a900898c1..c5f5791a92d 100644 --- a/src/main/target/GEPRCF405/target.c +++ b/src/main/target/GEPRCF405/target.c @@ -24,12 +24,8 @@ #include "drivers/timer.h" #include "drivers/timer_def.h" - - - - timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), diff --git a/src/main/target/GEPRCF722/target.c b/src/main/target/GEPRCF722/target.c index 851ae376a54..7061c09ffc5 100644 --- a/src/main/target/GEPRCF722/target.c +++ b/src/main/target/GEPRCF722/target.c @@ -27,7 +27,7 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), diff --git a/src/main/target/GEPRCF722_BT_HD/target.c b/src/main/target/GEPRCF722_BT_HD/target.c index d1f902eb698..e7a6f1b332d 100644 --- a/src/main/target/GEPRCF722_BT_HD/target.c +++ b/src/main/target/GEPRCF722_BT_HD/target.c @@ -25,10 +25,8 @@ #include "drivers/timer.h" #include "drivers/sensor.h" - - timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), diff --git a/src/main/target/GEPRC_F722_AIO/target.c b/src/main/target/GEPRC_F722_AIO/target.c index a8ee276fce0..60b6b359e89 100644 --- a/src/main/target/GEPRC_F722_AIO/target.c +++ b/src/main/target/GEPRC_F722_AIO/target.c @@ -27,7 +27,7 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0), diff --git a/src/main/target/HAKRCF405D/target.c b/src/main/target/HAKRCF405D/target.c index 664db476332..0679523028c 100644 --- a/src/main/target/HAKRCF405D/target.c +++ b/src/main/target/HAKRCF405D/target.c @@ -30,7 +30,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU600 timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 @@ -41,9 +41,9 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH1, PB8, TIM_USE_LED, 0, 0), // LED STRIP(2,6) - DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // PWM1 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // PWM2 - DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PWM3 + // DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // PWM1 + // DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // PWM2 + // DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PWM3 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/HAKRCF411D/target.c b/src/main/target/HAKRCF411D/target.c index 1423d2d1992..f4fddb456ac 100644 --- a/src/main/target/HAKRCF411D/target.c +++ b/src/main/target/HAKRCF411D/target.c @@ -23,7 +23,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 diff --git a/src/main/target/HAKRCKD722/target.c b/src/main/target/HAKRCKD722/target.c index 16757ff6416..e114f4410bf 100644 --- a/src/main/target/HAKRCKD722/target.c +++ b/src/main/target/HAKRCKD722/target.c @@ -30,7 +30,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42 BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 diff --git a/src/main/target/HGLRCF722/target.c b/src/main/target/HGLRCF722/target.c index 1ebbbf63373..e0e4a73e2ad 100644 --- a/src/main/target/HGLRCF722/target.c +++ b/src/main/target/HGLRCF722/target.c @@ -47,8 +47,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 UP1-6 D(1, 3, 2) DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 & softserial1 + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 + // DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 & softserial1 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.c b/src/main/target/IFLIGHTF4_SUCCEXD/target.c index ad9ab68c9ea..b79bc160750 100644 --- a/src/main/target/IFLIGHTF4_SUCCEXD/target.c +++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.c @@ -24,7 +24,7 @@ #include "drivers/pinio.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 0, 0), DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 0, 0), diff --git a/src/main/target/IFLIGHTF4_TWING/target.c b/src/main/target/IFLIGHTF4_TWING/target.c index 87227cf3b04..fcf8b09a85d 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.c +++ b/src/main/target/IFLIGHTF4_TWING/target.c @@ -30,7 +30,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu0, DEVHW_MPU6500, MPU6500_0_SPI_BUS, MPU6 BUSDEV_REGISTER_SPI_TAG(busdev_imu1, DEVHW_MPU6500, MPU6500_1_SPI_BUS, MPU6500_1_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_1_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM // Motors DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D2_ST6 diff --git a/src/main/target/IFLIGHTF7_TWING/target.c b/src/main/target/IFLIGHTF7_TWING/target.c index 6876f07b204..b04f6ee8d32 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.c +++ b/src/main/target/IFLIGHTF7_TWING/target.c @@ -30,7 +30,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu0, DEVHW_MPU6500, MPU6500_0_SPI_BUS, MPU6 BUSDEV_REGISTER_SPI_TAG(busdev_imu1, DEVHW_MPU6500, MPU6500_1_SPI_BUS, MPU6500_1_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_1_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), diff --git a/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c b/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c index 54ba97a4905..e46c36e0507 100644 --- a/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c +++ b/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.c @@ -30,7 +30,7 @@ //BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH2, PB3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM2, CH2, PB3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S1 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S2 diff --git a/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c b/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c index b4b6f6de0fb..664ff354b4c 100644 --- a/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c +++ b/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.c @@ -30,7 +30,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_ BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 diff --git a/src/main/target/IFLIGHT_JBF7PRO/target.c b/src/main/target/IFLIGHT_JBF7PRO/target.c index 676c3d6dae3..dcf7ae64385 100644 --- a/src/main/target/IFLIGHT_JBF7PRO/target.c +++ b/src/main/target/IFLIGHT_JBF7PRO/target.c @@ -25,7 +25,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 diff --git a/src/main/target/KAKUTEF4/target.c b/src/main/target/KAKUTEF4/target.c index bf58e5bdca8..ba47d09ffec 100755 --- a/src/main/target/KAKUTEF4/target.c +++ b/src/main/target/KAKUTEF4/target.c @@ -29,7 +29,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT - DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT - DMA1_ST2 diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c index 53ae9359deb..e8ea4230120 100755 --- a/src/main/target/KAKUTEF7/target.c +++ b/src/main/target/KAKUTEF7/target.c @@ -29,7 +29,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 + // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 diff --git a/src/main/target/KAKUTEF7MINIV3/target.c b/src/main/target/KAKUTEF7MINIV3/target.c index 095038960de..864c1de5204 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.c +++ b/src/main/target/KAKUTEF7MINIV3/target.c @@ -38,7 +38,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // RX2 + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // RX2 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KROOZX/target.c b/src/main/target/KROOZX/target.c index c77a77aaed5..a445a9656f6 100755 --- a/src/main/target/KROOZX/target.c +++ b/src/main/target/KROOZX/target.c @@ -23,7 +23,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // PPM IN DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 0, 0), // PWM4 DEF_TIM(TIM5, CH4, PA3, TIM_USE_MOTOR, 0, 0), // PWM2 diff --git a/src/main/target/MAMBAF405US/target.c b/src/main/target/MAMBAF405US/target.c index 1781a047e3f..19ba5944c20 100644 --- a/src/main/target/MAMBAF405US/target.c +++ b/src/main/target/MAMBAF405US/target.c @@ -25,7 +25,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN #ifdef MAMBAF405US_I2C DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1 pin A9: DMA2 Stream 6 Channel 0 diff --git a/src/main/target/MAMBAF722/target.c b/src/main/target/MAMBAF722/target.c index b97969e57bf..e1afa9b221f 100644 --- a/src/main/target/MAMBAF722/target.c +++ b/src/main/target/MAMBAF722/target.c @@ -25,7 +25,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT – D(2, 4, 7) DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT – D(2, 7, 7) diff --git a/src/main/target/MAMBAF722_2022A/target.c b/src/main/target/MAMBAF722_2022A/target.c index 9b51f139453..4d371135963 100644 --- a/src/main/target/MAMBAF722_2022A/target.c +++ b/src/main/target/MAMBAF722_2022A/target.c @@ -25,7 +25,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2 diff --git a/src/main/target/MAMBAF722_X8/target.c b/src/main/target/MAMBAF722_X8/target.c index 9b51f139453..4d371135963 100644 --- a/src/main/target/MAMBAF722_X8/target.c +++ b/src/main/target/MAMBAF722_X8/target.c @@ -25,7 +25,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2 diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 254707887b2..1706954b7f9 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -22,7 +22,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 UP(2,1) diff --git a/src/main/target/MATEKF405CAN/target.c b/src/main/target/MATEKF405CAN/target.c index 2ab40de1dc4..406a74ecd86 100644 --- a/src/main/target/MATEKF405CAN/target.c +++ b/src/main/target/MATEKF405CAN/target.c @@ -36,7 +36,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 + DEF_TIM(TIM9, CH2, PA3, TIM_USE_ANY, 0, 0), //RX2 DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; diff --git a/src/main/target/MATEKF405SE/target.c b/src/main/target/MATEKF405SE/target.c index ef6fceaca94..6aa586e68ae 100644 --- a/src/main/target/MATEKF405SE/target.c +++ b/src/main/target/MATEKF405SE/target.c @@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index 79f6269a528..2240915365a 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -42,7 +42,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; diff --git a/src/main/target/MATEKF411/target.c b/src/main/target/MATEKF411/target.c index fb2ccde3f14..9a15bfd8d1a 100755 --- a/src/main/target/MATEKF411/target.c +++ b/src/main/target/MATEKF411/target.c @@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,5,3) - clash with S2 DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), //softserial_tx2 - 2812LED TIM_USE_LED D(2,1,6) - DEF_TIM(TIM5, CH1, PA0, TIM_USE_PPM, 0, 0), //use rssi pad for PPM/softserial_tx1 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_ANY, 0, 0), //use rssi pad for PPM/softserial_tx1 //DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2 }; diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c index d2575f42f2d..eb4275d39f8 100644 --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -25,7 +25,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1, 4, 5) DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2, 3, 7) diff --git a/src/main/target/MATEKF722PX/target.c b/src/main/target/MATEKF722PX/target.c index 170b67d9514..da31a68e42d 100755 --- a/src/main/target/MATEKF722PX/target.c +++ b/src/main/target/MATEKF722PX/target.c @@ -40,10 +40,9 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 3, 6) - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // RX2, PPM - DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2, softserial1_tx - - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // Cam_ctrl reserved + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // RX2, PPM + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2, softserial1_tx + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // Cam_ctrl reserved }; diff --git a/src/main/target/MATEKF722SE/target.c b/src/main/target/MATEKF722SE/target.c index 6e469dfff3a..585f3bb3994 100644 --- a/src/main/target/MATEKF722SE/target.c +++ b/src/main/target/MATEKF722SE/target.c @@ -43,9 +43,9 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 & softserial1 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2 & softserial1 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF765/target.c b/src/main/target/MATEKF765/target.c index 712beb4c8c9..6d0599a29f9 100644 --- a/src/main/target/MATEKF765/target.c +++ b/src/main/target/MATEKF765/target.c @@ -52,8 +52,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_2812 D(2,6,0) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM - DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6 + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6 and SoftwareSerial DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM for MATEKF765SE }; diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 1923e6623a1..bc2fb01558a 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -51,10 +51,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM - DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6 - DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0, 0), // RX4 - DEF_TIM(TIM17, CH1, PB9, TIM_USE_ANY, 0, 0), // TX4 + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6 SoftwareSerial + // DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0, 0), // RX4 + // DEF_TIM(TIM17, CH1, PB9, TIM_USE_ANY, 0, 0), // TX4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NEUTRONRCF435SE/target.c b/src/main/target/NEUTRONRCF435SE/target.c index 3a8848911fa..54c4932ebe1 100644 --- a/src/main/target/NEUTRONRCF435SE/target.c +++ b/src/main/target/NEUTRONRCF435SE/target.c @@ -26,20 +26,19 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TMR5, CH1, PA0, TIM_USE_ANY, 0, 13), // TIM_USE_CAMERA_CONTROL - DEF_TIM(TMR5, CH2, PA1, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 - DEF_TIM(TMR2, CH4, PA3, TIM_USE_ANY |TIM_USE_PPM, 0,6), // PWM2 - PPM DMA1 CH6 - - DEF_TIM(TMR8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA2 CH7 - DEF_TIM(TMR8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0,2), // motor2 DMA2 CH6 - DEF_TIM(TMR3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0,1), // motor3 DMA2 CH5 - DEF_TIM(TMR3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0,3), // motor4 DMA2 CH4 - - DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,11), // PWM1 - OUT5 DMA1 CH7 - DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,10), // PWM2 - OUT6 DMA2 CH1 - DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,9), // PWM3 - OUT7 DMA2 CH2 - DEF_TIM(TMR3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0,8), // PWM4 - OUT8 DMA2 CH3 - + DEF_TIM(TMR8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA2 CH7 + DEF_TIM(TMR8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0,2), // motor2 DMA2 CH6 + DEF_TIM(TMR3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0,1), // motor3 DMA2 CH5 + DEF_TIM(TMR3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0,3), // motor4 DMA2 CH4 + + DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,11), // PWM1 - OUT5 DMA1 CH7 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,10), // PWM2 - OUT6 DMA2 CH1 + DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,9), // PWM3 - OUT7 DMA2 CH2 + DEF_TIM(TMR3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0,8), // PWM4 - OUT8 DMA2 CH3 + + // DEF_TIM(TMR5, CH1, PA0, TIM_USE_ANY, 0, 13), // TIM_USE_CAMERA_CONTROL + DEF_TIM(TMR5, CH2, PA1, TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 + // DEF_TIM(TMR2, CH4, PA3, TIM_USE_ANY |TIM_USE_PPM, 0,6), // PWM2 - PPM DMA1 CH6 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NOX/target.c b/src/main/target/NOX/target.c index e2651956e1c..647c77bfa11 100755 --- a/src/main/target/NOX/target.c +++ b/src/main/target/NOX/target.c @@ -24,8 +24,6 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), //PPM - DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), //2812LED DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT DEF_TIM(TIM1, CH1N, PA7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT @@ -34,6 +32,9 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH1, PA2, TIM_USE_ANY, 0, 0), //UART2 TX DEF_TIM(TIM9, CH2, PA3, TIM_USE_ANY, 0, 0), //UART2 RX + + // DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), //PPM + DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), //2812LED }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 81b30e11f25..86485aa49fc 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -24,17 +24,6 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) - DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN -#else - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN -#endif - DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX - DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO - DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 @@ -63,6 +52,19 @@ timerHardware_t timerHardware[] = { #if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) #endif + +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) + // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM4, CH4, PB9, TIM_USE_ANY, 0, 0), // S2_IN +#else + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM12, CH2, PB15, TIM_USE_ANY, 0, 0), // S2_IN +#endif + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX + DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO + }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUSF7NXT/target.c b/src/main/target/OMNIBUSF7NXT/target.c index b1eb7332dca..573c3e1bcd0 100644 --- a/src/main/target/OMNIBUSF7NXT/target.c +++ b/src/main/target/OMNIBUSF7NXT/target.c @@ -36,7 +36,7 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM / UART1_RX + // DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM / UART1_RX // OUTPUT 1-4 DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 1, 0), // D(1, 5, 5) diff --git a/src/main/target/PIXRACER/target.c b/src/main/target/PIXRACER/target.c index 73c9d6937a0..2364e60aec5 100755 --- a/src/main/target/PIXRACER/target.c +++ b/src/main/target/PIXRACER/target.c @@ -37,7 +37,7 @@ BUSDEV_REGISTER_I2C_TAG(busdev_mag3110, DEVHW_MAG3110, MAG_I2C_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_hmc5983_spi, DEVHW_HMC5883, MPU9250_SPI_BUS, PE15, NONE, 1, DEVFLAGS_NONE, 0); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PPM, 0, 0), // PPM shared uart6 pc7 + // DEF_TIM(TIM3, CH3, PB0, TIM_USE_PPM, 0, 0), // PPM shared uart6 pc7 DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT diff --git a/src/main/target/RADIX/target.c b/src/main/target/RADIX/target.c index db78409cdaf..ba10f018188 100644 --- a/src/main/target/RADIX/target.c +++ b/src/main/target/RADIX/target.c @@ -25,7 +25,7 @@ /* TIMERS */ timerHardware_t timerHardware[] = { - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM In + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PPM, 0, 0), // PPM In DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index d1203adadc5..e1a2c2e8418 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -41,12 +41,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO | TIM_USE_ANY, 0, 0), // S5_OUT / LED DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT D1_ST2 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S3_IN - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S4_IN - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S5_IN - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_OUTPUT_AUTO, 0, 0), // S6_IN + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PPM (5th pin on FlexiIO port) + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_IN + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_IN + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_IN }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SAGEATF4/target.c b/src/main/target/SAGEATF4/target.c index 48875414672..dd0f274b9f4 100644 --- a/src/main/target/SAGEATF4/target.c +++ b/src/main/target/SAGEATF4/target.c @@ -26,9 +26,7 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TMR5, CH4, PA3, TIM_USE_PPM, 0, 4), // PPM UART2_RX_PIN DMA1 CH5 - DEF_TIM(TMR2, CH3, PB10, TIM_USE_ANY | TIM_USE_LED, 0,5), // PWM1 - LED MCO1 DMA1 CH6 - DEF_TIM(TMR2, CH4, PB11, TIM_USE_ANY | TIM_USE_BEEPER, 0,6), // PWM2 - BB DMA1 CH7 + // DEF_TIM(TMR5, CH4, PA3, TIM_USE_PPM, 0, 4), // PPM UART2_RX_PIN DMA1 CH5 DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA1 CH1 DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,1), // motor2 DMA1 CH2 @@ -40,6 +38,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0,10), // PWM3 - OUT7 DMA2 CH4 DEF_TIM(TMR3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,11), // PWM4 - OUT8 DMA2 CH5 + DEF_TIM(TMR2, CH3, PB10, TIM_USE_ANY | TIM_USE_LED, 0,5), // PWM1 - LED MCO1 DMA1 CH6 + DEF_TIM(TMR2, CH4, PB11, TIM_USE_ANY | TIM_USE_BEEPER, 0,6), // PWM2 - BB DMA1 CH7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SKYSTARSF405HD/target.c b/src/main/target/SKYSTARSF405HD/target.c index 40ab18015af..3da0f45db92 100644 --- a/src/main/target/SKYSTARSF405HD/target.c +++ b/src/main/target/SKYSTARSF405HD/target.c @@ -22,7 +22,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), diff --git a/src/main/target/SKYSTARSF722HD/target.c b/src/main/target/SKYSTARSF722HD/target.c index 72ac8974552..bee870243af 100644 --- a/src/main/target/SKYSTARSF722HD/target.c +++ b/src/main/target/SKYSTARSF722HD/target.c @@ -22,7 +22,7 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM3, CH1, PB4, TIM_USE_PPM, 0, 0), DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // TIM3_CH3 / TIM8_CH3 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // TIM3_CH4 / TIM8_CH4 diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c index b4b4b295047..9364f231c54 100644 --- a/src/main/target/SPARKY2/target.c +++ b/src/main/target/SPARKY2/target.c @@ -24,11 +24,11 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM IN - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM, 0, 0 ), // S2_IN - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S3_IN - GPIO_PartialRemap_TIM3 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S4_IN - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S5_IN + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM, 0, 0 ), // S2_IN + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S3_IN - GPIO_PartialRemap_TIM3 + // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S4_IN + // DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S5_IN DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT diff --git a/src/main/target/SPEEDYBEEF4/target.c b/src/main/target/SPEEDYBEEF4/target.c index b45c517bfaa..075640d0f05 100644 --- a/src/main/target/SPEEDYBEEF4/target.c +++ b/src/main/target/SPEEDYBEEF4/target.c @@ -24,7 +24,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 UP(1,2) DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 UP(2,1) diff --git a/src/main/target/SPEEDYBEEF7/target.c b/src/main/target/SPEEDYBEEF7/target.c index 78821f52594..ac38530b84e 100644 --- a/src/main/target/SPEEDYBEEF7/target.c +++ b/src/main/target/SPEEDYBEEF7/target.c @@ -24,7 +24,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 diff --git a/src/main/target/SPEEDYBEEF7MINI/target.c b/src/main/target/SPEEDYBEEF7MINI/target.c index 32b5792dda2..2b0777ab573 100644 --- a/src/main/target/SPEEDYBEEF7MINI/target.c +++ b/src/main/target/SPEEDYBEEF7MINI/target.c @@ -46,7 +46,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED D(2, 6, 0) - DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1 + // DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1 DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 & softserial1 }; diff --git a/src/main/target/SPEEDYBEEF7V2/target.c b/src/main/target/SPEEDYBEEF7V2/target.c index 0914224a05b..abf06021a14 100644 --- a/src/main/target/SPEEDYBEEF7V2/target.c +++ b/src/main/target/SPEEDYBEEF7V2/target.c @@ -27,7 +27,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 diff --git a/src/main/target/SPRACINGF4EVO/target.c b/src/main/target/SPRACINGF4EVO/target.c index deabe8665d6..5b108168c1e 100755 --- a/src/main/target/SPRACINGF4EVO/target.c +++ b/src/main/target/SPRACINGF4EVO/target.c @@ -23,8 +23,8 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // PPM / PWM1 / UART2 RX - DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PPM / PWM2 / UART2 TX + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // PPM / PWM1 / UART2 RX + // DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PPM / PWM2 / UART2 TX DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // ESC 1 DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // ESC 2 diff --git a/src/main/target/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c index f47d1a08355..e94835b93c2 100644 --- a/src/main/target/SPRACINGF7DUAL/target.c +++ b/src/main/target/SPRACINGF7DUAL/target.c @@ -35,8 +35,8 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500_2, DEVHW_MPU6500, MPU6500_2_SPI_ timerHardware_t timerHardware[] = { - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // PPM / PWM1 / UART2 RX - DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PPM / PWM2 / UART2 TX + // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // PPM / PWM1 / UART2 RX + // DEF_TIM(TIM9, CH1, PA2, TIM_USE_PWM, 0, 0), // PPM / PWM2 / UART2 TX #if (SPRACINGF7DUAL_REV <= 1) DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), // ESC 1 diff --git a/src/main/target/TMOTORF7/target.c b/src/main/target/TMOTORF7/target.c index 2f96517ba15..0af36aa68ce 100644 --- a/src/main/target/TMOTORF7/target.c +++ b/src/main/target/TMOTORF7/target.c @@ -37,7 +37,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), - DEF_TIM(TIM2, CH1, PA15, TIM_USE_PPM, 0, 0), + // DEF_TIM(TIM2, CH1, PA15, TIM_USE_PPM, 0, 0), DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0) diff --git a/src/main/target/TMOTORF7V2/target.c b/src/main/target/TMOTORF7V2/target.c index 606ac8819dc..c21b0f0229a 100644 --- a/src/main/target/TMOTORF7V2/target.c +++ b/src/main/target/TMOTORF7V2/target.c @@ -24,7 +24,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 310ab66b0aa..837647a89b6 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -24,7 +24,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0 ), // PPM IN + // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 0 ), // PPM IN DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT - DMA1_ST2 DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - DMA1_ST4 diff --git a/src/main/target/YUPIF7/target.c b/src/main/target/YUPIF7/target.c index cd6a17ee683..3587a8bc4bc 100644 --- a/src/main/target/YUPIF7/target.c +++ b/src/main/target/YUPIF7/target.c @@ -24,7 +24,7 @@ #include "drivers/bus.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 1 ), // PPM IN + // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, 0, 1 ), // PPM IN DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT - DMA1_ST2 DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - DMA1_ST4 DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - DMA1_ST1 From a47afe2187f7120ef2462b049ab21f7817595c3e Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 6 Nov 2023 21:49:20 +0000 Subject: [PATCH 129/199] Show system message when linear descent begins --- src/main/io/osd.c | 17 ++++++++++++++--- src/main/io/osd.h | 1 + src/main/navigation/navigation.c | 5 +++++ src/main/navigation/navigation_private.h | 1 + 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1662094cabb..89ee0642a48 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -154,8 +154,9 @@ #define OSD_MIN_FONT_VERSION 3 -static timeMs_t notify_settings_saved = 0; -static bool savingSettings = false; +static timeMs_t linearDescentMessageMs = 0; +static timeMs_t notify_settings_saved = 0; +static bool savingSettings = false; static unsigned currentLayout = 0; static int layoutOverride = -1; @@ -1011,7 +1012,13 @@ static const char * navigationStateMessage(void) if (posControl.flags.rthTrackbackActive) { return OSD_MESSAGE_STR(OSD_MSG_RTH_TRACKBACK); } else { - return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); + if (posControl.rthState.rthLinearDescentActive && (linearDescentMessageMs == 0 || linearDescentMessageMs > millis())) { + if (linearDescentMessageMs == 0) + linearDescentMessageMs = millis() + 5000; // Show message for 5 seconds. + + return OSD_MESSAGE_STR(OSD_MSG_RTH_LINEAR_DESCENT); + } else + return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); } case MW_NAV_STATE_HOLD_INFINIT: // Used by HOLD flight modes. No information to add. @@ -1052,6 +1059,10 @@ static const char * navigationStateMessage(void) // Not used break; } + + if (!posControl.rthState.rthLinearDescentActive && linearDescentMessageMs != 0) + linearDescentMessageMs = 0; + return NULL; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index f09c9d049b3..18851c5d804 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -93,6 +93,7 @@ #define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" #define OSD_MSG_RTH_TRACKBACK "RTH BACK TRACKING" #define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" +#define OSD_MSG_RTH_LINEAR_DESCENT "BEGIN LINEAR DESCENT" #define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" #define OSD_MSG_WP_LANDED "WP END>LANDED" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c991531f32b..db04d38bd70 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1432,6 +1432,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio if (homeDistance <= METERS_TO_CENTIMETERS(navConfig()->general.rth_linear_descent_start_distance)) { posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude; + posControl.rthState.rthLinearDescentActive = true; } } @@ -1442,6 +1443,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio if (isWaypointReached(tmpHomePos, 0)) { // Successfully reached position target - update XYZ-position setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + + if (navConfig()->general.flags.rth_use_linear_descent) + posControl.rthState.rthLinearDescentActive = false; + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING } else { setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index c408f109c9b..776eca78ea3 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -350,6 +350,7 @@ typedef struct { float rthInitialDistance; // Distance when starting flight home fpVector3_t homeTmpWaypoint; // Temporary storage for home target fpVector3_t originalHomePosition; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET + bool rthLinearDescentActive; // Activation status of Linear Descent } rthState_t; typedef enum { From 62d78ae7c2e6b8ed72e9651ca5aa4b24d62ea76b Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 7 Nov 2023 08:13:35 +0000 Subject: [PATCH 130/199] Bug fix Multiple RTH and early exit from RTH fixed. Message would only be displayed for first RTH. --- src/main/io/osd.c | 6 +++--- src/main/navigation/navigation.c | 5 ++++- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 89ee0642a48..a178d5b55f1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1001,6 +1001,9 @@ static const char * divertingToSafehomeMessage(void) static const char * navigationStateMessage(void) { + if (!posControl.rthState.rthLinearDescentActive && linearDescentMessageMs != 0) + linearDescentMessageMs = 0; + switch (NAV_Status.state) { case MW_NAV_STATE_NONE: break; @@ -1060,9 +1063,6 @@ static const char * navigationStateMessage(void) break; } - if (!posControl.rthState.rthLinearDescentActive && linearDescentMessageMs != 0) - linearDescentMessageMs = 0; - return NULL; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index db04d38bd70..94014e86c48 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1227,6 +1227,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati { UNUSED(previousState); + if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) + posControl.rthState.rthLinearDescentActive = false; + if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable. @@ -1444,7 +1447,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // Successfully reached position target - update XYZ-position setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - if (navConfig()->general.flags.rth_use_linear_descent) + if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) posControl.rthState.rthLinearDescentActive = false; return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING From c6d15733e289e53f4ae5510c8ccbfa90bc0a8c1a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 7 Nov 2023 10:56:04 +0100 Subject: [PATCH 131/199] change to ANYFCF7 --- src/main/target/ANYFCF7/target.c | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index dc523f4c780..b5ef1f12c4a 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -26,21 +26,23 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN - DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST0 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT - DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT 4 DMA1_ST1 DMA1_ST3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT 3 DMA1_ST4 - DEF_TIM(TIM9, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT DMA1_ST5 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7_OUT DMA1_ST2 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S8_OUT DMA1_ST6 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S9_OUT DMA1_ST4 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_IN DMA2_ST7 + + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST0 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1_OUT 4 DMA1_ST1 DMA1_ST3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT 3 DMA1_ST4 + DEF_TIM(TIM9, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT DMA1_ST5 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7_OUT DMA1_ST2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S8_OUT DMA1_ST6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S9_OUT DMA1_ST4 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_IN DMA2_ST7 + }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); From b471fc529aaaeb42448af76e7b58da55278ba34d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 7 Nov 2023 20:23:59 +0100 Subject: [PATCH 132/199] F411 PSA --- readme.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/readme.md b/readme.md index 046cff3404d..724f1a88c14 100644 --- a/readme.md +++ b/readme.md @@ -1,5 +1,11 @@ # INAV - navigation capable flight controller +# PSA + +> INAV no longer accepts targets based on STM32 F411 MCU. + +> INAV 7 is the last INAV official release available for F411 based flight controllers. The next milestone, INAV 8 will not be available for F411 boards. + ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png) # INAV Community From 745017d058f6929e52fa97750ae0179d920095bb Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 8 Nov 2023 19:03:41 +0900 Subject: [PATCH 133/199] Add fade out effect to baro altitude correction --- src/main/navigation/navigation_pos_estimator.c | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 8157fa730cd..f66c641928b 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -590,9 +590,14 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) bool correctOK = false; - const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; //ignore baro if difference is too big, baro is probably wrong + //ignore baro if difference is too big, baro is probably wrong + const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; + //fade out the baro to prevent sudden jump + const float start_epv = positionEstimationConfig()->max_eph_epv; + const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f; + const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f); //use both baro and gps - if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv * 2)) { + if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -616,14 +621,14 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) // Altitude const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z; - ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; + ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; + ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); + ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); } correctOK = true; From 5cab1b1820aae7ab2863f4cb658300d48237fa61 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 8 Nov 2023 12:12:39 +0100 Subject: [PATCH 134/199] Change the SPI of Mamba H743 --- .vscode/settings.json | 3 ++- src/main/target/MAMBAH743/target.h | 15 +++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index ebbda008b64..9be3fced22b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -4,7 +4,8 @@ "cmath": "c", "ranges": "c", "platform.h": "c", - "timer.h": "c" + "timer.h": "c", + "bus.h": "c" }, "editor.tabSize": 4, "editor.insertSpaces": true, diff --git a/src/main/target/MAMBAH743/target.h b/src/main/target/MAMBAH743/target.h index 18070e1775e..7fcea85f48b 100644 --- a/src/main/target/MAMBAH743/target.h +++ b/src/main/target/MAMBAH743/target.h @@ -60,16 +60,15 @@ #ifdef MAMBAH743_2022B -// SPI4 is used on the second MPU6000 gyro, we do not use it at the moment -// #define USE_SPI_DEVICE_4 -// #define SPI4_SCK_PIN PE12 -// #define SPI4_MISO_PIN PE13 -// #define SPI4_MOSI_PIN PE14 +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 #define USE_IMU_ICM42605 -#define IMU_ICM42605_ALIGN CW0_DEG -#define ICM42605_SPI_BUS BUS_SPI1 -#define ICM42605_CS_PIN PA4 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_SPI_BUS BUS_SPI4 +#define ICM42605_CS_PIN PE11 #endif From 5c0c3ddb7dc0c4c468bddd27c81c0d3097ecaaad Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 8 Nov 2023 18:43:27 +0100 Subject: [PATCH 135/199] Change 3d deadband check to use 3d deadband min and max from ui, instead of mid_throttle_deadband --- src/main/fc/rc_controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 8a03ed81284..7dec6d624a8 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -119,8 +119,8 @@ throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e value = rcCommand[THROTTLE]; } - const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband; - bool midThrottle = value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband); + //const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband; + bool midThrottle = value > (rcControlsConfig->deadband_low) && value < (rcControlsConfig->deadband_high); if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) { return THROTTLE_LOW; } From 27b0b49d1bbcfc499abafc6c4ee79577a2aeacc8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 8 Nov 2023 18:44:44 +0100 Subject: [PATCH 136/199] remove unused variable --- src/main/fc/rc_controls.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 7dec6d624a8..c9208106f9f 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -119,7 +119,6 @@ throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e value = rcCommand[THROTTLE]; } - //const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband; bool midThrottle = value > (rcControlsConfig->deadband_low) && value < (rcControlsConfig->deadband_high); if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) { return THROTTLE_LOW; From b41b05715c4a7c65e4d87dfac807831d1937cd3a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 8 Nov 2023 18:54:29 +0100 Subject: [PATCH 137/199] Use correct method to read config --- src/main/fc/rc_controls.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index c9208106f9f..9cce60a2a38 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -119,7 +119,7 @@ throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e value = rcCommand[THROTTLE]; } - bool midThrottle = value > (rcControlsConfig->deadband_low) && value < (rcControlsConfig->deadband_high); + bool midThrottle = value > (reversibleMotorsConfig()->deadband_low) && value < (reversibleMotorsConfig()->deadband_high); if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) { return THROTTLE_LOW; } From 9c01d065b3aec3a51a82b01d8952bf6467774c43 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 8 Nov 2023 18:57:28 +0000 Subject: [PATCH 138/199] Update navigation_fixedwing.c --- src/main/navigation/navigation_fixedwing.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e6084e0972e..86746023c77 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -518,8 +518,8 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs) // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs // FIXME: verify the above calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2); - updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate); + needToCalculateCircularLoiter = false; } else { // Position update has not occurred in time (first iteration or glitch), reset altitude controller From 4df28d0dbda920472287cad2dd3cf635aaa991a9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 9 Nov 2023 12:52:26 +0100 Subject: [PATCH 139/199] Fix gyro orientation --- src/main/target/ATOMRCF405NAVI_DELUX/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/ATOMRCF405NAVI_DELUX/target.h b/src/main/target/ATOMRCF405NAVI_DELUX/target.h index 1134daa29f1..f97d5d92e69 100644 --- a/src/main/target/ATOMRCF405NAVI_DELUX/target.h +++ b/src/main/target/ATOMRCF405NAVI_DELUX/target.h @@ -65,7 +65,7 @@ //ICM42688-P #define USE_IMU_ICM42605 -#define IMU_ICM42605_ALIGN CW180_DEG +#define IMU_ICM42605_ALIGN CW270_DEG #define ICM42605_CS_PIN PA4 #define ICM42605_SPI_BUS BUS_SPI1 From a8e090d4a9551eaa76374e8c037b4a02ff0f0007 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 9 Nov 2023 19:21:15 +0100 Subject: [PATCH 140/199] This board has an optional baro Since the baro is not always populated, it needs more drivers. --- src/main/target/TMOTORF7V2/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/TMOTORF7V2/target.h b/src/main/target/TMOTORF7V2/target.h index 525330e120c..2691812f2fc 100644 --- a/src/main/target/TMOTORF7V2/target.h +++ b/src/main/target/TMOTORF7V2/target.h @@ -95,7 +95,7 @@ // BMP280 BARO --- I2C1 #define USE_I2C #define USE_BARO -#define USE_BARO_BMP280 +#define USE_BARO_ALL #define BARO_I2C_BUS BUS_I2C1 #define USE_I2C_DEVICE_1 From 297bded4d8ebffa80fdf24be6ebf8b704815acb7 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 10 Nov 2023 10:06:02 +0100 Subject: [PATCH 141/199] Revert "Enable the virtual pitot by default" This reverts commit 83844e996fbd229a16dc83e76e553fcf297ed5c8. --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 17384f92732..a7783356695 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5058,7 +5058,7 @@ Selection of pitot hardware. | Default | Min | Max | | --- | --- | --- | -| VIRTUAL | | | +| NONE | | | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1011638959d..805c94b2893 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -588,7 +588,7 @@ groups: members: - name: pitot_hardware description: "Selection of pitot hardware." - default_value: "VIRTUAL" + default_value: "NONE" table: pitot_hardware - name: pitot_lpf_milli_hz min: 0 From 1f3d41b33ea8add3e3e28eb8b93d8ff0baacab19 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 10 Nov 2023 10:21:48 +0100 Subject: [PATCH 142/199] It looks like user actually had a v1 board --- src/main/target/TMOTORF7/target.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/target/TMOTORF7/target.h b/src/main/target/TMOTORF7/target.h index 9e935d1dc67..068894323d9 100644 --- a/src/main/target/TMOTORF7/target.h +++ b/src/main/target/TMOTORF7/target.h @@ -52,9 +52,7 @@ #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 -#define USE_BARO_BMP280 -#define USE_BARO_MS5611 -#define USE_BARO_DPS310 +#define USE_BARO_ALL #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 From 78ebfeb0119e1790aea1da955eeb36ddd653f55e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 10 Nov 2023 20:01:21 +0100 Subject: [PATCH 143/199] Change timer allocation default to MOTORS This board has an unfortunate timer allocation, where the first 4 outputs use 3 timers. This causes the default allocation to be M1, M2, M3, S1, M4 Fixes #9474 --- src/main/target/SPEEDYBEEF7V3/config.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/SPEEDYBEEF7V3/config.c b/src/main/target/SPEEDYBEEF7V3/config.c index b064cd99f96..903ca93f95b 100644 --- a/src/main/target/SPEEDYBEEF7V3/config.c +++ b/src/main/target/SPEEDYBEEF7V3/config.c @@ -31,4 +31,7 @@ void targetConfiguration(void) { serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_ESCSERIAL; pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; } From c1b8b5b80b0e702da4c0540653d690d581aab28d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 10 Nov 2023 20:06:54 +0100 Subject: [PATCH 144/199] DAKEFPVF722 target --- src/main/target/DAKEFPVF722/CMakeLists.txt | 1 + src/main/target/DAKEFPVF722/config.c | 28 ++++ src/main/target/DAKEFPVF722/target.c | 36 +++++ src/main/target/DAKEFPVF722/target.h | 150 +++++++++++++++++++++ 4 files changed, 215 insertions(+) create mode 100644 src/main/target/DAKEFPVF722/CMakeLists.txt create mode 100644 src/main/target/DAKEFPVF722/config.c create mode 100644 src/main/target/DAKEFPVF722/target.c create mode 100644 src/main/target/DAKEFPVF722/target.h diff --git a/src/main/target/DAKEFPVF722/CMakeLists.txt b/src/main/target/DAKEFPVF722/CMakeLists.txt new file mode 100644 index 00000000000..e2385c70c51 --- /dev/null +++ b/src/main/target/DAKEFPVF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(DAKEFPVF722) diff --git a/src/main/target/DAKEFPVF722/config.c b/src/main/target/DAKEFPVF722/config.c new file mode 100644 index 00000000000..bfb7ffbef3c --- /dev/null +++ b/src/main/target/DAKEFPVF722/config.c @@ -0,0 +1,28 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include + +#include "io/serial.h" +#include "rx/rx.h" + +void targetConfiguration(void) +{ +} diff --git a/src/main/target/DAKEFPVF722/target.c b/src/main/target/DAKEFPVF722/target.c new file mode 100644 index 00000000000..8f31bd7350d --- /dev/null +++ b/src/main/target/DAKEFPVF722/target.c @@ -0,0 +1,36 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/DAKEFPVF722/target.h b/src/main/target/DAKEFPVF722/target.h new file mode 100644 index 00000000000..92cc65783b8 --- /dev/null +++ b/src/main/target/DAKEFPVF722/target.h @@ -0,0 +1,150 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "DAK7" +#define USBD_PRODUCT_STRING "DAKEFPV F722" + +#define LED0 PC14 +#define LED1 PC15 + +#define BEEPER PC3 +#define BEEPER_INVERTED + +// Buses +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +// Gyro +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW90_DEG +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_BUS BUS_SPI1 + +//Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define BMP280_SPI_BUS BUS_SPI2 +#define BMP280_CS_PIN PA13 + +// M25P256 flash +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// Serial ports +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +// ADC +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +#define USE_LED_STRIP +#define WS2811_PIN PB3 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define MAX_PWM_OUTPUT_PORTS 6 From 17150f73701e14711d4410706859f9fcfcc3cb24 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 10 Nov 2023 20:10:03 +0100 Subject: [PATCH 145/199] Add missing #include --- src/main/target/SPEEDYBEEF7V3/config.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/SPEEDYBEEF7V3/config.c b/src/main/target/SPEEDYBEEF7V3/config.c index 903ca93f95b..9bc0a52a898 100644 --- a/src/main/target/SPEEDYBEEF7V3/config.c +++ b/src/main/target/SPEEDYBEEF7V3/config.c @@ -26,6 +26,7 @@ #include "fc/fc_msp_box.h" #include "io/serial.h" +#include "drivers/pwm_mapping.h" void targetConfiguration(void) { From c8e383a4d9680ebd0a360bc5f7cf66b02beea318 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 10 Nov 2023 20:46:08 +0100 Subject: [PATCH 146/199] DAKEFPVF405 target --- src/main/target/DAKEFPVF405/CMakeLists.txt | 1 + src/main/target/DAKEFPVF405/config.c | 31 ++++ src/main/target/DAKEFPVF405/target.c | 36 +++++ src/main/target/DAKEFPVF405/target.h | 168 +++++++++++++++++++++ 4 files changed, 236 insertions(+) create mode 100644 src/main/target/DAKEFPVF405/CMakeLists.txt create mode 100644 src/main/target/DAKEFPVF405/config.c create mode 100644 src/main/target/DAKEFPVF405/target.c create mode 100644 src/main/target/DAKEFPVF405/target.h diff --git a/src/main/target/DAKEFPVF405/CMakeLists.txt b/src/main/target/DAKEFPVF405/CMakeLists.txt new file mode 100644 index 00000000000..9a147fdb702 --- /dev/null +++ b/src/main/target/DAKEFPVF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(DAKEFPVF405) diff --git a/src/main/target/DAKEFPVF405/config.c b/src/main/target/DAKEFPVF405/config.c new file mode 100644 index 00000000000..73f636cf8c0 --- /dev/null +++ b/src/main/target/DAKEFPVF405/config.c @@ -0,0 +1,31 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include + +#include "io/serial.h" +#include "rx/rx.h" +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +} diff --git a/src/main/target/DAKEFPVF405/target.c b/src/main/target/DAKEFPVF405/target.c new file mode 100644 index 00000000000..8f31bd7350d --- /dev/null +++ b/src/main/target/DAKEFPVF405/target.c @@ -0,0 +1,36 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/DAKEFPVF405/target.h b/src/main/target/DAKEFPVF405/target.h new file mode 100644 index 00000000000..41c128668ef --- /dev/null +++ b/src/main/target/DAKEFPVF405/target.h @@ -0,0 +1,168 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "DAK4" +#define USBD_PRODUCT_STRING "DAKEFPV F405" + +#define LED0 PC14 +#define LED1 PC15 + +#define BEEPER PC3 +#define BEEPER_INVERTED + +// Buses +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +// Gyro +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW90_DEG +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_BUS BUS_SPI1 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW90_DEG +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + +//Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define BMP280_SPI_BUS BUS_SPI2 +#define BMP280_CS_PIN PC13 +#define USE_BARO_DPS310 +#define DPS310_SPI_BUS BUS_SPI2 +#define DPS310_CS_PIN PC13 + +// M25P256 flash +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// Serial ports +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +// ADC +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +#define USE_LED_STRIP +#define WS2811_PIN PB3 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC5 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED \ No newline at end of file From 36fdb7e2a01d05dcbd59f8c6f2a189032a1596d6 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 11 Nov 2023 12:06:09 +0100 Subject: [PATCH 147/199] Correct output order to match BF output order https://github.com/betaflight/unified-targets/blob/master/configs/default/FLWO-FLYWOOF745.config Reported by @sarcasticfpv on discord --- src/main/target/FLYWOOF745/target.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c index 7c79d1f6a52..5ad1d7dd035 100644 --- a/src/main/target/FLYWOOF745/target.c +++ b/src/main/target/FLYWOOF745/target.c @@ -32,11 +32,11 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2 DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4 DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5 From 434ab87c43aa88a6ca31bb0e6bc9d97d4a4bc4d6 Mon Sep 17 00:00:00 2001 From: Sensei Date: Sat, 11 Nov 2023 05:15:40 -0600 Subject: [PATCH 148/199] Flywoof745 of der commented motor numbers Updates the // M1, // m2 etc to match the new order. --- src/main/target/FLYWOOF745/target.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c index 5ad1d7dd035..dcdfc8654f6 100644 --- a/src/main/target/FLYWOOF745/target.c +++ b/src/main/target/FLYWOOF745/target.c @@ -32,11 +32,11 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA2_ST7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 , DMA1_ST7 DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M5 , DMA2_ST2 DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4 DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5 From a7e29116881e37fc2ee3338bacb2dbddb9df5b4c Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 11 Nov 2023 12:17:23 +0100 Subject: [PATCH 149/199] Fix motor labels on target.c and make sure M1-M4 land on S1-S4 by default --- src/main/target/FLYWOOF745/config.c | 9 +++++++++ src/main/target/FLYWOOF745/target.c | 10 +++++----- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/main/target/FLYWOOF745/config.c b/src/main/target/FLYWOOF745/config.c index 07f6de46971..7dfe40728dd 100644 --- a/src/main/target/FLYWOOF745/config.c +++ b/src/main/target/FLYWOOF745/config.c @@ -22,7 +22,16 @@ #include "io/piniobox.h" +#include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" + void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + + // Make sure S1-S4 default to Motors + + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM1))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c index 5ad1d7dd035..a82a708f51a 100644 --- a/src/main/target/FLYWOOF745/target.c +++ b/src/main/target/FLYWOOF745/target.c @@ -32,15 +32,15 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA2_ST7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 , DMA1_ST7 DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M5 , DMA2_ST2 DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4 DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); From 10226d298fa7f172dc76ae39f51f2577923b225b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 12 Nov 2023 12:11:50 +0100 Subject: [PATCH 150/199] Update output assignment --- src/main/target/ATOMRCF405NAVI_DELUX/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/ATOMRCF405NAVI_DELUX/target.c b/src/main/target/ATOMRCF405NAVI_DELUX/target.c index e3ff9d8f77e..54d715d8cbf 100644 --- a/src/main/target/ATOMRCF405NAVI_DELUX/target.c +++ b/src/main/target/ATOMRCF405NAVI_DELUX/target.c @@ -46,9 +46,9 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 - DEF_TIM(TIM8, CH2N, PB14, TIM_USE_SERVO, 0, 0), // S10 + DEF_TIM(TIM8, CH2N, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 - DEF_TIM(TIM1, CH3N, PB15, TIM_USE_SERVO, 0, 0), // S11 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_STRIP }; From 92327688937535591479c45c52d48c5ce429598e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 15 Nov 2023 09:26:57 +0100 Subject: [PATCH 151/199] Update current scale --- src/main/target/ATOMRCF405NAVI_DELUX/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/ATOMRCF405NAVI_DELUX/target.h b/src/main/target/ATOMRCF405NAVI_DELUX/target.h index f97d5d92e69..f04a5d5da6f 100644 --- a/src/main/target/ATOMRCF405NAVI_DELUX/target.h +++ b/src/main/target/ATOMRCF405NAVI_DELUX/target.h @@ -131,7 +131,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_3 #define AIRSPEED_ADC_CHANNEL ADC_CHN_4 -#define CURRENT_METER_SCALE 320 +#define CURRENT_METER_SCALE 128 /* * OSD From a2a05629a0f0bd250b7601d98ff48e8b43b1913a Mon Sep 17 00:00:00 2001 From: error414 Date: Mon, 13 Nov 2023 19:30:50 +0100 Subject: [PATCH 152/199] fix hdzero channel settings over displayport --- src/main/fc/fc_msp.c | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index fddcde00ebf..79e7418ced9 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2581,6 +2581,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (sbufBytesRemaining(src) > 0) { vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); } + + // ////////////////////////////////////////////////////////// + // this code is taken from BF, it's hack for HDZERO VTX MSP frame + // API version 1.42 - this parameter kept separate since clients may already be supplying + if (sbufBytesRemaining(src) >= 2) { + sbufReadU16(src); //skip pitModeFreq + } + + // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency + if (sbufBytesRemaining(src) >= 4) { + uint8_t newBand = sbufReadU8(src); + const uint8_t newChannel = sbufReadU8(src); + vtxSettingsConfigMutable()->band = newBand; + vtxSettingsConfigMutable()->channel = newChannel; + } + + /* if (sbufBytesRemaining(src) >= 4) { + sbufRead8(src); // freq_l + sbufRead8(src); // freq_h + sbufRead8(src); // band count + sbufRead8(src); // channel count + }*/ + // ////////////////////////////////////////////////////////// } } } From dce738842536ec2680e4a43da467c002e6213544 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 15 Nov 2023 15:21:51 -0600 Subject: [PATCH 153/199] FLYWOOF405S_AIO: rename CMakeLists --- src/main/target/FLYWOOF405S_AIO/{CMakeLists => CMakeLists.txt} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/target/FLYWOOF405S_AIO/{CMakeLists => CMakeLists.txt} (100%) diff --git a/src/main/target/FLYWOOF405S_AIO/CMakeLists b/src/main/target/FLYWOOF405S_AIO/CMakeLists.txt similarity index 100% rename from src/main/target/FLYWOOF405S_AIO/CMakeLists rename to src/main/target/FLYWOOF405S_AIO/CMakeLists.txt From 5b832e9c10f18fc4cc6cdbfe5bbea0610ab80548 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 5 Nov 2023 14:09:45 +0100 Subject: [PATCH 154/199] IFLIGHT_BLITZ_ATF435 initial commit --- .../IFLIGHT_BLITZ_ATF435/CMakeLists.txt | 1 + src/main/target/IFLIGHT_BLITZ_ATF435/target.c | 45 +++++ src/main/target/IFLIGHT_BLITZ_ATF435/target.h | 157 ++++++++++++++++++ 3 files changed, 203 insertions(+) create mode 100644 src/main/target/IFLIGHT_BLITZ_ATF435/CMakeLists.txt create mode 100644 src/main/target/IFLIGHT_BLITZ_ATF435/target.c create mode 100644 src/main/target/IFLIGHT_BLITZ_ATF435/target.h diff --git a/src/main/target/IFLIGHT_BLITZ_ATF435/CMakeLists.txt b/src/main/target/IFLIGHT_BLITZ_ATF435/CMakeLists.txt new file mode 100644 index 00000000000..c0136c9630c --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_ATF435/CMakeLists.txt @@ -0,0 +1 @@ +target_at32f43x_xGT7(IFLIGHT_BLITZ_ATF435) \ No newline at end of file diff --git a/src/main/target/IFLIGHT_BLITZ_ATF435/target.c b/src/main/target/IFLIGHT_BLITZ_ATF435/target.c new file mode 100644 index 00000000000..2639b718744 --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_ATF435/target.c @@ -0,0 +1,45 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TMR3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0,8), // S1 + DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,9), // S2 + DEF_TIM(TMR8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0,2), // S3 + DEF_TIM(TMR8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,0), // S4 + + DEF_TIM(TMR1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + diff --git a/src/main/target/IFLIGHT_BLITZ_ATF435/target.h b/src/main/target/IFLIGHT_BLITZ_ATF435/target.h new file mode 100644 index 00000000000..7518bbe13e2 --- /dev/null +++ b/src/main/target/IFLIGHT_BLITZ_ATF435/target.h @@ -0,0 +1,157 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "IAT4" + +#define USBD_PRODUCT_STRING "iFlight BLITZ ATF435" + +#define LED0 PC15 + +#define BEEPER PB2 +#define BEEPER_INVERTED + + +// Buses + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +// #define USE_I2C_PULLUP + +#define DEFAULT_I2C_BUS BUS_I2C1 + +// Gyro + +// BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PA4 + +// Other sensors + +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +// OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// Flash + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PA15 + +// UARTs +#define USE_VCP +// #define USB_DETECT_PIN PC5 +// #define USE_USB_DETECT + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC1_DMA_STREAM DMA2_CHANNEL5 + +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) + +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE BIT(2) + +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_DSHOT +#define USE_ESC_SENSOR From db7350e13f1fbb442eb9c58c5dc0fd60546b8e5d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 14 Nov 2023 18:45:52 +0100 Subject: [PATCH 155/199] Outputs update --- src/main/target/IFLIGHT_BLITZ_ATF435/target.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/target/IFLIGHT_BLITZ_ATF435/target.c b/src/main/target/IFLIGHT_BLITZ_ATF435/target.c index 2639b718744..2de0fd50578 100644 --- a/src/main/target/IFLIGHT_BLITZ_ATF435/target.c +++ b/src/main/target/IFLIGHT_BLITZ_ATF435/target.c @@ -38,6 +38,11 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0,2), // S3 DEF_TIM(TMR8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,0), // S4 + DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,11), // S5 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,10), // S6 + DEF_TIM(TMR2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0,5), //S7 + DEF_TIM(TMR2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0,6), //S8 + DEF_TIM(TMR1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP }; From d1658da5e84662106b08f42856a02dade6e355b8 Mon Sep 17 00:00:00 2001 From: druckgott Date: Sat, 18 Nov 2023 20:01:24 +0100 Subject: [PATCH 156/199] add some icons for O3 with integra for ESP32 Radar instead of ? Add some icons for ESP32Radar instead of have ?. Update displayport_msp_bf_compat.c fix some ; --- src/main/io/displayport_msp_bf_compat.c | 42 +++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 4229e035b03..dd120a2295d 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -578,6 +578,48 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case (SYM_AH_V_START+4): case (SYM_AH_V_START+5): return BF_SYM_AH_BAR9_4; + + // BF for ESP_RADAR Symbols + case SYM_HUD_CARDINAL: + return BF_SYM_ARROW_SOUTH; + case SYM_HUD_CARDINAL + 1: + return BF_SYM_ARROW_16; + case SYM_HUD_CARDINAL + 2: + return BF_SYM_ARROW_15; + case SYM_HUD_CARDINAL + 3: + return BF_SYM_ARROW_WEST; + case SYM_HUD_CARDINAL + 4: + return BF_SYM_ARROW_12; + case SYM_HUD_CARDINAL + 5: + return BF_SYM_ARROW_11; + case SYM_HUD_CARDINAL + 6: + return BF_SYM_ARROW_NORTH; + case SYM_HUD_CARDINAL + 7: + return BF_SYM_ARROW_7; + case SYM_HUD_CARDINAL + 8: + return BF_SYM_ARROW_6; + case SYM_HUD_CARDINAL + 9: + return BF_SYM_ARROW_EAST; + case SYM_HUD_CARDINAL + 10: + return BF_SYM_ARROW_3; + case SYM_HUD_CARDINAL + 11: + return BF_SYM_ARROW_2; + + case SYM_HUD_ARROWS_R3: + return BF_SYM_AH_RIGHT; + case SYM_HUD_ARROWS_L3: + return BF_SYM_AH_LEFT; + + case SYM_HUD_SIGNAL_0: + return BF_SYM_AH_BAR9_1; + case SYM_HUD_SIGNAL_1: + return BF_SYM_AH_BAR9_3; + case SYM_HUD_SIGNAL_2: + return BF_SYM_AH_BAR9_4; + case SYM_HUD_SIGNAL_3: + return BF_SYM_AH_BAR9_5; + case SYM_HUD_SIGNAL_4: + return BF_SYM_AH_BAR9_7; /* case SYM_VARIO_UP_2A: return BF_SYM_VARIO_UP_2A; From f64639a0607bd032db364a2612a95b77bdfaf471 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 19 Nov 2023 18:51:55 +0000 Subject: [PATCH 157/199] Add param to delay landing in event of a Failsafe --- src/main/fc/settings.yaml | 6 ++++++ src/main/navigation/navigation.c | 20 ++++++++++++++++++-- src/main/navigation/navigation.h | 1 + 3 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4f870fa8f7a..94ec5995c22 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2592,6 +2592,12 @@ groups: default_value: "ALWAYS" field: general.flags.rth_allow_landing table: nav_rth_allow_landing + - name: nav_rth_fs_landing_delay + description: "If landing is active on Failsafe and this is above 0. The aircraft will hover or loiter for X seconds before performing the landing. If the battery enters the warning or critical levels, the land will proceed. Default = 0 [seconds]" + default_value: 0 + min: 0 + max: 1800 + field: general.rth_fs_landing_delay - name: nav_rth_alt_mode description: "Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details" default_value: "AT_LEAST" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c566c94bfad..e61fed07e9a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -155,6 +155,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT, .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps + .rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing }, // MC-specific @@ -225,6 +226,7 @@ static navWapointHeading_t wpHeadingControl; navigationPosControl_t posControl; navSystemStatus_t NAV_Status; static bool landingDetectorIsActive; +static timeMs_t landingDelay = 0; EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; @@ -1470,9 +1472,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - // If position ok OR within valid timeout - continue + // Action delay before landing if in FS and option enabled + bool pauseLanding = false; + if (NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE) && navConfig()->general.rth_fs_landing_delay > 0) { + if (landingDelay == 0) + landingDelay = millis() + S2MS(navConfig()->general.rth_fs_landing_delay); + + batteryState_e batteryState = getBatteryState(); + + if (millis() < landingDelay && batteryState != BATTERY_WARNING && batteryState != BATTERY_CRITICAL) + pauseLanding = true; + else + landingDelay = 0; + } + + // If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing - if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { + if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) { resetLandingDetector(); // force reset landing detector just in case updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d4ee6e078ed..2308effddff 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -268,6 +268,7 @@ typedef struct navConfig_s { uint16_t auto_disarm_delay; // safety time delay for landing detector uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately) uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled + uint16_t rth_fs_landing_delay; // Delay upon reaching home before starting landing if in FS (0 = immediate) } general; struct { From 9b92d85d1e75c4b4e72d9f086c27d4c072113b2b Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 19 Nov 2023 19:00:14 +0000 Subject: [PATCH 158/199] Update Settings.md --- docs/Settings.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index 17384f92732..e5131122a79 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3822,6 +3822,16 @@ If set to ON, aircraft will execute initial climb regardless of position sensor --- +### nav_rth_fs_landing_delay + +If landing is active on Failsafe and this is above 0. The aircraft will hover or loiter for X seconds before performing the landing. If the battery enters the warning or critical levels, the land will proceed. Default = 0 [seconds] + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1800 | + +--- + ### nav_rth_home_altitude Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] From 1af7e38e5cdd4c5efec4a7207bc64db7085f59d1 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 19 Nov 2023 19:16:48 +0000 Subject: [PATCH 159/199] Bug fix --- src/main/navigation/navigation.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e61fed07e9a..bfa69308771 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1474,7 +1474,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND // Action delay before landing if in FS and option enabled bool pauseLanding = false; - if (NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE) && navConfig()->general.rth_fs_landing_delay > 0) { + navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; + if ((allow == NAV_RTH_ALLOW_LANDING_ALWAYS || allow == NAV_RTH_ALLOW_LANDING_FS_ONLY) && FLIGHT_MODE(FAILSAFE_MODE) && navConfig()->general.rth_fs_landing_delay > 0) { if (landingDelay == 0) landingDelay = millis() + S2MS(navConfig()->general.rth_fs_landing_delay); From b4847bd3622866dba45ab15aec6cccb12a24889f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 20 Nov 2023 22:01:07 +0000 Subject: [PATCH 160/199] add BB wp number --- src/main/blackbox/blackbox.c | 4 ++++ src/main/navigation/navigation.c | 5 +++++ src/main/navigation/navigation.h | 3 ++- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index cb12a7a8968..5a5d88cb9c3 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -436,6 +436,7 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)}, #endif {"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)}, + {"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, }; typedef enum BlackboxState { @@ -554,6 +555,7 @@ typedef struct blackboxSlowState_s { int8_t escTemperature; #endif uint16_t rxUpdateRate; + uint8_t activeWpNumber; } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From rc_controls.c @@ -1291,6 +1293,7 @@ static void writeSlowFrame(void) blackboxWriteSignedVB(slowHistory.escTemperature); #endif blackboxWriteUnsignedVB(slowHistory.rxUpdateRate); + blackboxWriteUnsignedVB(slowHistory.activeWpNumber); blackboxSlowFrameIterationTimer = 0; } @@ -1368,6 +1371,7 @@ static void loadSlowState(blackboxSlowState_t *slow) #endif slow->rxUpdateRate = getRcUpdateFrequency(); + slow->activeWpNumber = getActiveWpNumber(); } /** diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c566c94bfad..5e7f22665ac 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4583,3 +4583,8 @@ int8_t navCheckActiveAngleHoldAxis(void) return activeAxis; } + +uint8_t getActiveWpNumber(void) +{ + return NAV_Status.activeWpNumber; +} diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d4ee6e078ed..a939c623ef6 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -595,7 +595,7 @@ bool isFixedWingAutoThrottleManuallyIncreased(void); bool navigationIsFlyingAutonomousMode(void); bool navigationIsExecutingAnEmergencyLanding(void); bool navigationIsControllingAltitude(void); -/* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS +/* Returns true if navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. */ bool navigationRTHAllowsLanding(void); @@ -628,6 +628,7 @@ void checkManualEmergencyLandingControl(bool forcedActivation); float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue); int8_t navCheckActiveAngleHoldAxis(void); +uint8_t getActiveWpNumber(void); /* Returns the heading recorded when home position was acquired. * Note that the navigation system uses deg*100 as unit and angles From 56dd1a1157f8319332647c46c20a8dde0fe75747 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 21 Nov 2023 20:12:38 +0100 Subject: [PATCH 161/199] submit-remove-vtx-freq-artefacts --- docs/Settings.md | 4 ++-- src/main/drivers/vtx_common.h | 4 ---- src/main/fc/settings.yaml | 4 ++-- 3 files changed, 4 insertions(+), 8 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 17384f92732..3c431bd4a5d 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5994,11 +5994,11 @@ Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, ### vtx_band -Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. +Configure the VTX band. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | Default | Min | Max | | --- | --- | --- | -| 1 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND | +| 1 | VTX_SETTINGS_MIN_BAND | VTX_SETTINGS_MAX_BAND | --- diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index ed4d5e251a1..021d17f24de 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -19,7 +19,6 @@ #include "common/time.h" -#define VTX_SETTINGS_NO_BAND 0 // used for custom frequency selection mode #define VTX_SETTINGS_MIN_BAND 1 #define VTX_SETTINGS_MAX_BAND 5 #define VTX_SETTINGS_MIN_CHANNEL 1 @@ -33,9 +32,6 @@ #define VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL 1 #define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0 -#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting -#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting - #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) #define VTX_SETTINGS_POWER_COUNT 5 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4f870fa8f7a..0c517ab8c57 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3810,10 +3810,10 @@ groups: condition: USE_VTX_SMARTAUDIO || USE_VTX_TRAMP members: - name: vtx_band - description: "Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race." + description: "Configure the VTX band. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race." default_value: 1 field: band - min: VTX_SETTINGS_NO_BAND + min: VTX_SETTINGS_MIN_BAND max: VTX_SETTINGS_MAX_BAND - name: vtx_channel description: "Channel to use within the configured `vtx_band`. Valid values are [1, 8]." From 3d2e1028467c136430b66a43ff9afa7cd7f5508d Mon Sep 17 00:00:00 2001 From: Sensei Date: Wed, 22 Nov 2023 21:03:24 -0600 Subject: [PATCH 162/199] Update Boards.md: Add link to full list --- docs/Boards.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docs/Boards.md b/docs/Boards.md index 9440058758c..a20f9908cb0 100644 --- a/docs/Boards.md +++ b/docs/Boards.md @@ -12,6 +12,9 @@ These boards are well tested with INAV and are known to be of good quality and r | [Holybro Kakute H7](https://inavflight.com/shop/s/bg/1914066) | H7 | KAKUTEH7 | All | All | All | All | All | SERIAL, SD | It's possible to find more supported and tested boards [here](https://github.com/iNavFlight/inav/wiki/Welcome-to-INAV,-useful-links-and-products) + +There is also [full list of all upported boards].(https://github.com/iNavFlight/inav/wiki/Boards,-Targets-and-PWM-allocations) + ### Boards documentation See the [docs/boards](https://github.com/iNavFlight/inav/tree/master/docs/boards) folder for additional information regards to many targets in INAV, to example help in finding pinout and features. _Feel free to help improve the docs._ From 5ac78bd926d7e3035daef275d47396f86a64e210 Mon Sep 17 00:00:00 2001 From: Sensei Date: Wed, 22 Nov 2023 21:06:48 -0600 Subject: [PATCH 163/199] Boards.md move punctuation --- docs/Boards.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Boards.md b/docs/Boards.md index a20f9908cb0..f61f3a2fd4a 100644 --- a/docs/Boards.md +++ b/docs/Boards.md @@ -13,7 +13,7 @@ These boards are well tested with INAV and are known to be of good quality and r It's possible to find more supported and tested boards [here](https://github.com/iNavFlight/inav/wiki/Welcome-to-INAV,-useful-links-and-products) -There is also [full list of all upported boards].(https://github.com/iNavFlight/inav/wiki/Boards,-Targets-and-PWM-allocations) +There is also a [full list of all supported boards](https://github.com/iNavFlight/inav/wiki/Boards,-Targets-and-PWM-allocations). ### Boards documentation From 84d0e46c5583d71e9edd302bdc0abfc1645d21f5 Mon Sep 17 00:00:00 2001 From: YI-BOYANG <46438966+YI-BOYANG@users.noreply.github.com> Date: Thu, 23 Nov 2023 17:40:29 +0800 Subject: [PATCH 164/199] Add IIC2 and change the barometer to IIC2 --- src/main/target/GEPRCF405/target.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/target/GEPRCF405/target.h b/src/main/target/GEPRCF405/target.h index 7f641643e88..0b96d5b8b1e 100644 --- a/src/main/target/GEPRCF405/target.h +++ b/src/main/target/GEPRCF405/target.h @@ -61,9 +61,12 @@ #define USE_I2C_DEVICE_1 #define I2C1_SCL PB8 #define I2C1_SDA PB9 +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 #define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 +#define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_BMP280 #define USE_BARO_DPS310 #define USE_BARO_MS5611 From e34faeed92572a5ecfcbe41fb0fa9a387240f683 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 25 Nov 2023 11:44:21 +0000 Subject: [PATCH 165/199] Added landing delay message --- src/main/io/osd.c | 6 ++++++ src/main/navigation/navigation.c | 10 +++++----- src/main/navigation/navigation_private.h | 1 + 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b799ab5c562..9cd875d7175 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1047,6 +1047,7 @@ static const char * navigationStateMessage(void) case MW_NAV_STATE_LANDED: return OSD_MESSAGE_STR(OSD_MSG_LANDED); case MW_NAV_STATE_LAND_SETTLE: + // If there is a FS landing delay occurring. That is handled by the calling function. return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND); case MW_NAV_STATE_LAND_START_DESCENT: // Not used @@ -5124,6 +5125,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = messageBuf; } + } else if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) { + uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis()); + tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec); + + messages[messageCount++] = messageBuf; } else { const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bfa69308771..762be575757 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -226,7 +226,6 @@ static navWapointHeading_t wpHeadingControl; navigationPosControl_t posControl; navSystemStatus_t NAV_Status; static bool landingDetectorIsActive; -static timeMs_t landingDelay = 0; EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; @@ -1444,6 +1443,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio if (isWaypointReached(tmpHomePos, 0)) { // Successfully reached position target - update XYZ-position setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + posControl.landingDelay = 0; return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING } else { setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); @@ -1476,15 +1476,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND bool pauseLanding = false; navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; if ((allow == NAV_RTH_ALLOW_LANDING_ALWAYS || allow == NAV_RTH_ALLOW_LANDING_FS_ONLY) && FLIGHT_MODE(FAILSAFE_MODE) && navConfig()->general.rth_fs_landing_delay > 0) { - if (landingDelay == 0) - landingDelay = millis() + S2MS(navConfig()->general.rth_fs_landing_delay); + if (posControl.landingDelay == 0) + posControl.landingDelay = millis() + S2MS(navConfig()->general.rth_fs_landing_delay); batteryState_e batteryState = getBatteryState(); - if (millis() < landingDelay && batteryState != BATTERY_WARNING && batteryState != BATTERY_CRITICAL) + if (millis() < posControl.landingDelay && batteryState != BATTERY_WARNING && batteryState != BATTERY_CRITICAL) pauseLanding = true; else - landingDelay = 0; + posControl.landingDelay = 0; } // If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index c408f109c9b..8e8c2e3446d 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -394,6 +394,7 @@ typedef struct { rthState_t rthState; uint32_t homeDistance; // cm int32_t homeDirection; // deg*100 + timeMs_t landingDelay; /* Safehome parameters */ safehomeState_t safehomeState; From ec3c2c4228d678fe6e52d6b64598ad4dc4528f75 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 25 Nov 2023 11:55:55 +0000 Subject: [PATCH 166/199] Added precursor to future PR Currently the standard system message function is blocked if any messages in the if section (osd.c:5108-5138) are used. I plan on unblocking the standard messages. --- src/main/io/osd.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9cd875d7175..a3538da3f8e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1047,8 +1047,13 @@ static const char * navigationStateMessage(void) case MW_NAV_STATE_LANDED: return OSD_MESSAGE_STR(OSD_MSG_LANDED); case MW_NAV_STATE_LAND_SETTLE: + { // If there is a FS landing delay occurring. That is handled by the calling function. + if (posControl.landingDelay > 0) + break; + return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND); + } case MW_NAV_STATE_LAND_START_DESCENT: // Not used break; From 5c12057e6c79f6501599b101e120cf6986f7d410 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 29 Nov 2023 12:30:41 +0000 Subject: [PATCH 167/199] Additional Description for Control Derivative --- docs/Settings.md | 8 ++++---- src/main/fc/settings.yaml | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3c431bd4a5d..39e4eaac2bf 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2444,7 +2444,7 @@ This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK ### mc_cd_lpf_hz -Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky +Cutoff frequency for Control Derivative. This controls the cutoff for the LPF that is applied to the CD (Feed Forward) signal to the PID controller. Lower value will produce a smoother CD gain to the controller, but it will be more delayed. Higher values will produce CD gain that may have more noise in the signal depending on your RC link but wil be less delayed. | Default | Min | Max | | --- | --- | --- | @@ -2454,7 +2454,7 @@ Cutoff frequency for Control Derivative. Lower value smoother reaction on fast s ### mc_cd_pitch -Multicopter Control Derivative gain for PITCH +Multicopter Control Derivative gain for PITCH. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. | Default | Min | Max | | --- | --- | --- | @@ -2464,7 +2464,7 @@ Multicopter Control Derivative gain for PITCH ### mc_cd_roll -Multicopter Control Derivative gain for ROLL +Multicopter Control Derivative gain for ROLL. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. | Default | Min | Max | | --- | --- | --- | @@ -2474,7 +2474,7 @@ Multicopter Control Derivative gain for ROLL ### mc_cd_yaw -Multicopter Control Derivative gain for YAW +Multicopter Control Derivative gain for YAW. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0c517ab8c57..44f1f6c24b5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1730,7 +1730,7 @@ groups: min: RPYL_PID_MIN max: RPYL_PID_MAX - name: mc_cd_pitch - description: "Multicopter Control Derivative gain for PITCH" + description: "Multicopter Control Derivative gain for PITCH. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." default_value: 60 field: bank_mc.pid[PID_PITCH].FF min: RPYL_PID_MIN @@ -1754,7 +1754,7 @@ groups: min: RPYL_PID_MIN max: RPYL_PID_MAX - name: mc_cd_roll - description: "Multicopter Control Derivative gain for ROLL" + description: "Multicopter Control Derivative gain for ROLL. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." default_value: 60 field: bank_mc.pid[PID_ROLL].FF min: RPYL_PID_MIN @@ -1778,7 +1778,7 @@ groups: min: RPYL_PID_MIN max: RPYL_PID_MAX - name: mc_cd_yaw - description: "Multicopter Control Derivative gain for YAW" + description: "Multicopter Control Derivative gain for YAW. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." default_value: 60 field: bank_mc.pid[PID_YAW].FF min: RPYL_PID_MIN @@ -2195,7 +2195,7 @@ groups: table: pidTypeTable default_value: AUTO - name: mc_cd_lpf_hz - description: "Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky" + description: "Cutoff frequency for Control Derivative. This controls the cutoff for the LPF that is applied to the CD (Feed Forward) signal to the PID controller. Lower value will produce a smoother CD gain to the controller, but it will be more delayed. Higher values will produce CD gain that may have more noise in the signal depending on your RC link but wil be less delayed." default_value: 30 field: controlDerivativeLpfHz min: 0 From 3d2078e1343fe284ec3a44390b75cc659bb49963 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 30 Nov 2023 08:29:28 +0000 Subject: [PATCH 168/199] added hint for BetaFlight users --- docs/Settings.md | 6 +++--- src/main/fc/settings.yaml | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 39e4eaac2bf..02b66c2cd9c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2454,7 +2454,7 @@ Cutoff frequency for Control Derivative. This controls the cutoff for the LPF th ### mc_cd_pitch -Multicopter Control Derivative gain for PITCH. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. +Multicopter Control Derivative gain for PITCH (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. | Default | Min | Max | | --- | --- | --- | @@ -2464,7 +2464,7 @@ Multicopter Control Derivative gain for PITCH. The CD intoduces a term to the PI ### mc_cd_roll -Multicopter Control Derivative gain for ROLL. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. +Multicopter Control Derivative gain for ROLL (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. | Default | Min | Max | | --- | --- | --- | @@ -2474,7 +2474,7 @@ Multicopter Control Derivative gain for ROLL. The CD intoduces a term to the PID ### mc_cd_yaw -Multicopter Control Derivative gain for YAW. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. +Multicopter Control Derivative gain for YAW (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 44f1f6c24b5..cc46ab0df8f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1730,7 +1730,7 @@ groups: min: RPYL_PID_MIN max: RPYL_PID_MAX - name: mc_cd_pitch - description: "Multicopter Control Derivative gain for PITCH. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." + description: "Multicopter Control Derivative gain for PITCH (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." default_value: 60 field: bank_mc.pid[PID_PITCH].FF min: RPYL_PID_MIN @@ -1754,7 +1754,7 @@ groups: min: RPYL_PID_MIN max: RPYL_PID_MAX - name: mc_cd_roll - description: "Multicopter Control Derivative gain for ROLL. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." + description: "Multicopter Control Derivative gain for ROLL (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." default_value: 60 field: bank_mc.pid[PID_ROLL].FF min: RPYL_PID_MIN @@ -1778,7 +1778,7 @@ groups: min: RPYL_PID_MIN max: RPYL_PID_MAX - name: mc_cd_yaw - description: "Multicopter Control Derivative gain for YAW. The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." + description: "Multicopter Control Derivative gain for YAW (known as 'Feed Forward' in Betaflight). The CD intoduces a term to the PID controller that is the magnitude of the Setpoint change. Fast inputs produce a high CD gain to help push the MC into a move; in advance of the P-gain if set high enough." default_value: 60 field: bank_mc.pid[PID_YAW].FF min: RPYL_PID_MIN From dce928823e8422309cc5d21b24370d51fd117c8a Mon Sep 17 00:00:00 2001 From: jhemcu <1553061986@qq.com> Date: Mon, 4 Dec 2023 20:41:59 +0800 Subject: [PATCH 169/199] target: Add JHEMCU F722 targer board support --- src/main/target/JHEMCUF722/CMakeLists.txt | 1 + src/main/target/JHEMCUF722/config.c | 28 ++++ src/main/target/JHEMCUF722/target.c | 45 ++++++ src/main/target/JHEMCUF722/target.h | 182 ++++++++++++++++++++++ 4 files changed, 256 insertions(+) create mode 100644 src/main/target/JHEMCUF722/CMakeLists.txt create mode 100644 src/main/target/JHEMCUF722/config.c create mode 100644 src/main/target/JHEMCUF722/target.c create mode 100644 src/main/target/JHEMCUF722/target.h diff --git a/src/main/target/JHEMCUF722/CMakeLists.txt b/src/main/target/JHEMCUF722/CMakeLists.txt new file mode 100644 index 00000000000..abbb496eafd --- /dev/null +++ b/src/main/target/JHEMCUF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(JHEMCUF722) diff --git a/src/main/target/JHEMCUF722/config.c b/src/main/target/JHEMCUF722/config.c new file mode 100644 index 00000000000..caaec66ef23 --- /dev/null +++ b/src/main/target/JHEMCUF722/config.c @@ -0,0 +1,28 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; // VTX power switch + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/JHEMCUF722/target.c b/src/main/target/JHEMCUF722/target.c new file mode 100644 index 00000000000..68450216a0e --- /dev/null +++ b/src/main/target/JHEMCUF722/target.c @@ -0,0 +1,45 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0 ), //PPM&SBUS + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 DMA1_S7_CH5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 - D(1,2) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 - D(1,4) + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 - D(1,6) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 - D(2,1) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 - D(2,4) + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP(1,5) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/JHEMCUF722/target.h b/src/main/target/JHEMCUF722/target.h new file mode 100644 index 00000000000..9d34e5c8201 --- /dev/null +++ b/src/main/target/JHEMCUF722/target.h @@ -0,0 +1,182 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "JHF7" +#define USBD_PRODUCT_STRING "JHEMCUF722" + +#define LED0 PA15 + +#define USE_BEEPER +#define BEEPER PC15 +#define BEEPER_INVERTED + +// PINIO +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC14 +#define PINIO2_PIN PB9 + +// UART +#define USB_IO +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +// Gyro & Acc +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 +#define ICM42605_EXTI_PIN PC3 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW90_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PA4 +#define BMI270_EXTI_PIN PC3 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW90_DEG +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_EXTI_PIN PC3 + +// I2C (Baro & Mag) +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +// Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 +#define BARO_I2C_BUS BUS_I2C1 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +// Onboard Flash +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PC13 + +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PC13 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define USE_FLASH_W25N01G +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// OSD +#define USE_OSD +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// ADC +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// LED +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +// Optical Flow & Lidar +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +// Misc +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define SERIALRX_UART SERIAL_PORT_USART2 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define CURRENT_METER_SCALE 250 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) From 93b82f8c960ce825b2e7add14648c6a5126c31bd Mon Sep 17 00:00:00 2001 From: jhemcu <1553061986@qq.com> Date: Mon, 4 Dec 2023 20:44:35 +0800 Subject: [PATCH 170/199] target: Add JHEMCU F405 targer board support --- src/main/target/JHEMCUF405/CMakeLists.txt | 1 + src/main/target/JHEMCUF405/target.c | 39 +++++ src/main/target/JHEMCUF405/target.h | 164 ++++++++++++++++++++++ 3 files changed, 204 insertions(+) create mode 100644 src/main/target/JHEMCUF405/CMakeLists.txt create mode 100644 src/main/target/JHEMCUF405/target.c create mode 100644 src/main/target/JHEMCUF405/target.h diff --git a/src/main/target/JHEMCUF405/CMakeLists.txt b/src/main/target/JHEMCUF405/CMakeLists.txt new file mode 100644 index 00000000000..1c8253eb464 --- /dev/null +++ b/src/main/target/JHEMCUF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(JHEMCUF405) diff --git a/src/main/target/JHEMCUF405/target.c b/src/main/target/JHEMCUF405/target.c new file mode 100644 index 00000000000..463a5cbc0e6 --- /dev/null +++ b/src/main/target/JHEMCUF405/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,3,2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,0,2) + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 D(1,7,5) + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1,2,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(2,4,7) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(2,7,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), // 2812LED D(1,5,3) + DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/JHEMCUF405/target.h b/src/main/target/JHEMCUF405/target.h new file mode 100644 index 00000000000..6a5b2f52ab4 --- /dev/null +++ b/src/main/target/JHEMCUF405/target.h @@ -0,0 +1,164 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "JH45" +#define USBD_PRODUCT_STRING "JHEMCUF405" + +#define LED0 PC14 //Green +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW90_DEG +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW90_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PB12 +#define BMI270_EXTI_PIN GYRO_INT_EXTI + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_CS_PIN PB12 +#define ICM42605_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PB13 +#define USE_MPU_DATA_READY_SIGNAL + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +// *************** SPI2 OSD *************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PB14 + +// *************** Onboard flash ******************** +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_BUS BUS_SPI3 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_PIN PA8 +#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** LED2812 ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA9 + +// *************** OTHERS ************************* +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) +#define CURRENT_METER_SCALE 250 + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 8 From 4788513a05cb7511503bb9ba86e4df9d35136027 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 4 Dec 2023 20:12:46 +0100 Subject: [PATCH 171/199] flashhobby targets --- src/main/target/FLASHHOBBYF405/CMakeLists.txt | 1 + src/main/target/FLASHHOBBYF405/target.c | 43 +++++ src/main/target/FLASHHOBBYF405/target.h | 164 ++++++++++++++++++ src/main/target/FLASHHOBBYF722/CMakeLists.txt | 1 + src/main/target/FLASHHOBBYF722/target.c | 45 +++++ src/main/target/FLASHHOBBYF722/target.h | 162 +++++++++++++++++ 6 files changed, 416 insertions(+) create mode 100644 src/main/target/FLASHHOBBYF405/CMakeLists.txt create mode 100644 src/main/target/FLASHHOBBYF405/target.c create mode 100644 src/main/target/FLASHHOBBYF405/target.h create mode 100644 src/main/target/FLASHHOBBYF722/CMakeLists.txt create mode 100644 src/main/target/FLASHHOBBYF722/target.c create mode 100644 src/main/target/FLASHHOBBYF722/target.h diff --git a/src/main/target/FLASHHOBBYF405/CMakeLists.txt b/src/main/target/FLASHHOBBYF405/CMakeLists.txt new file mode 100644 index 00000000000..7fa38644d1f --- /dev/null +++ b/src/main/target/FLASHHOBBYF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(FLASHHOBBYF405) diff --git a/src/main/target/FLASHHOBBYF405/target.c b/src/main/target/FLASHHOBBYF405/target.c new file mode 100644 index 00000000000..b088049a07b --- /dev/null +++ b/src/main/target/FLASHHOBBYF405/target.c @@ -0,0 +1,43 @@ +/* +* This file is part of INAV Project. +* +* Cleanflight is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Cleanflight is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +timerHardware_t timerHardware[] = { + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 1), + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1),//WS2812B + // DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // "C.C" +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file diff --git a/src/main/target/FLASHHOBBYF405/target.h b/src/main/target/FLASHHOBBYF405/target.h new file mode 100644 index 00000000000..937917c95c1 --- /dev/null +++ b/src/main/target/FLASHHOBBYF405/target.h @@ -0,0 +1,164 @@ +/* + * This file is part of INAV Project. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FHRCF405" + +#define USBD_PRODUCT_STRING "FLASHHOBBYF405" + +#define LED0 PC14 + +#define BEEPER PB2 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_CS_PIN SPI1_NSS_PIN +#define BMI270_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_CS_PIN SPI1_NSS_PIN +#define ICM42605_SPI_BUS BUS_SPI1 + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +// *************** FLASH ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN NONE + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +// *************** PINIO ************************ +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC13 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/FLASHHOBBYF722/CMakeLists.txt b/src/main/target/FLASHHOBBYF722/CMakeLists.txt new file mode 100644 index 00000000000..90566a14d66 --- /dev/null +++ b/src/main/target/FLASHHOBBYF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(FLASHHOBBYF722) diff --git a/src/main/target/FLASHHOBBYF722/target.c b/src/main/target/FLASHHOBBYF722/target.c new file mode 100644 index 00000000000..31a8f73544c --- /dev/null +++ b/src/main/target/FLASHHOBBYF722/target.c @@ -0,0 +1,45 @@ +/* +* This file is part of INAV Project. +* +* Cleanflight is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Cleanflight is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +timerHardware_t timerHardware[] = { + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 1), + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1),//WS2812B + // DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // "C.C" +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + + diff --git a/src/main/target/FLASHHOBBYF722/target.h b/src/main/target/FLASHHOBBYF722/target.h new file mode 100644 index 00000000000..f0cc7881352 --- /dev/null +++ b/src/main/target/FLASHHOBBYF722/target.h @@ -0,0 +1,162 @@ +/* + * This file is part of INAV Project. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FHRCF722" +#define USBD_PRODUCT_STRING "FLASHHOBBYF722" + +#define LED0 PC14 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_CS_PIN SPI1_NSS_PIN +#define BMI270_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_CS_PIN SPI1_NSS_PIN +#define ICM42605_SPI_BUS BUS_SPI1 + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// *************** FLASH ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN NONE + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +// *************** PINIO ************************ +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC13 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define USE_DSHOT +#define USE_ESC_SENSOR From 038b5a90eec2a0841de54778eb1a03300fc6dec2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 4 Dec 2023 23:40:12 +0100 Subject: [PATCH 172/199] Enable dshot --- src/main/target/BETAFPVF435/target.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/target/BETAFPVF435/target.h b/src/main/target/BETAFPVF435/target.h index 2376afbcfec..fa025b9d7fd 100644 --- a/src/main/target/BETAFPVF435/target.h +++ b/src/main/target/BETAFPVF435/target.h @@ -31,7 +31,7 @@ //#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO //#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_AUTO -//#define ENABLE_DSHOT +#define ENABLE_DSHOT // *************** Gyro & ACC ********************** #define USE_SPI @@ -177,7 +177,7 @@ #define USE_LED_STRIP #define WS2811_PIN PB6 -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP ) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP ) #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC @@ -189,11 +189,13 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE BIT(2) +#define TARGET_IO_PORTH (BIT(1)|BIT(2)|BIT(3)) #define MAX_PWM_OUTPUT_PORTS 8 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -//#define USE_DSHOT -//#define USE_ESC_SENSOR +#define USE_DSHOT +#define USE_ESC_SENSOR #define USE_ESCSERIAL +#define USE_RPM_FILTER From 39966a2261daf4514b73bc0e83096ca51c208fe9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 5 Dec 2023 18:44:36 +0100 Subject: [PATCH 173/199] SPEDIXF722 and SPEDIXF405 targets --- src/main/target/SPEDIXF405/CMakeLists.txt | 1 + src/main/target/SPEDIXF405/target.c | 45 ++++++ src/main/target/SPEDIXF405/target.h | 164 +++++++++++++++++++++ src/main/target/SPEDIXF722/CMakeLists.txt | 1 + src/main/target/SPEDIXF722/target.c | 45 ++++++ src/main/target/SPEDIXF722/target.h | 166 ++++++++++++++++++++++ 6 files changed, 422 insertions(+) create mode 100644 src/main/target/SPEDIXF405/CMakeLists.txt create mode 100644 src/main/target/SPEDIXF405/target.c create mode 100644 src/main/target/SPEDIXF405/target.h create mode 100644 src/main/target/SPEDIXF722/CMakeLists.txt create mode 100644 src/main/target/SPEDIXF722/target.c create mode 100644 src/main/target/SPEDIXF722/target.h diff --git a/src/main/target/SPEDIXF405/CMakeLists.txt b/src/main/target/SPEDIXF405/CMakeLists.txt new file mode 100644 index 00000000000..62ce61f7c7e --- /dev/null +++ b/src/main/target/SPEDIXF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(SPEDIXF405) diff --git a/src/main/target/SPEDIXF405/target.c b/src/main/target/SPEDIXF405/target.c new file mode 100644 index 00000000000..31a8f73544c --- /dev/null +++ b/src/main/target/SPEDIXF405/target.c @@ -0,0 +1,45 @@ +/* +* This file is part of INAV Project. +* +* Cleanflight is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Cleanflight is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +timerHardware_t timerHardware[] = { + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 1), + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1),//WS2812B + // DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // "C.C" +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + + diff --git a/src/main/target/SPEDIXF405/target.h b/src/main/target/SPEDIXF405/target.h new file mode 100644 index 00000000000..3f46890b48d --- /dev/null +++ b/src/main/target/SPEDIXF405/target.h @@ -0,0 +1,164 @@ +/* + * This file is part of INAV Project. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SPDXF405" + +#define USBD_PRODUCT_STRING "SPEDIXF405" + +#define LED0 PC14 + +#define BEEPER PB2 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_CS_PIN SPI1_NSS_PIN +#define BMI270_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_CS_PIN SPI1_NSS_PIN +#define ICM42605_SPI_BUS BUS_SPI1 + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN +// *************** FLASH ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN NONE + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +// *************** PINIO ************************ +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC13 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/SPEDIXF722/CMakeLists.txt b/src/main/target/SPEDIXF722/CMakeLists.txt new file mode 100644 index 00000000000..e937ebba9ea --- /dev/null +++ b/src/main/target/SPEDIXF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(SPEDIXF722) diff --git a/src/main/target/SPEDIXF722/target.c b/src/main/target/SPEDIXF722/target.c new file mode 100644 index 00000000000..31a8f73544c --- /dev/null +++ b/src/main/target/SPEDIXF722/target.c @@ -0,0 +1,45 @@ +/* +* This file is part of INAV Project. +* +* Cleanflight is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Cleanflight is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +timerHardware_t timerHardware[] = { + // DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 1), + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1),//WS2812B + // DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_FW_SERVO, 0, 0), // "C.C" +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + + diff --git a/src/main/target/SPEDIXF722/target.h b/src/main/target/SPEDIXF722/target.h new file mode 100644 index 00000000000..511381587a0 --- /dev/null +++ b/src/main/target/SPEDIXF722/target.h @@ -0,0 +1,166 @@ +/* + * This file is part of INAV Project. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SPDX722" + +#define USBD_PRODUCT_STRING "SPEDIXF722" + +#define LED0 PC14 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_CS_PIN SPI1_NSS_PIN +#define BMI270_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_CS_PIN SPI1_NSS_PIN +#define ICM42605_SPI_BUS BUS_SPI1 + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// *************** FLASH ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN NONE + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +// *************** PINIO ************************ +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC13 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define USE_DSHOT +#define USE_ESC_SENSOR From 0af8705b15b9d776778121da07e500dd4afc421d Mon Sep 17 00:00:00 2001 From: Vincent Poon Date: Mon, 4 Dec 2023 11:31:58 +0800 Subject: [PATCH 174/199] Add ICM42688 to Kakutef7miniv3 target.h Holybro will use ICM42688 to replace MPU6000 on this board. --- src/main/target/KAKUTEF7MINIV3/target.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h index 997cad1a976..bf485ebc82b 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.h +++ b/src/main/target/KAKUTEF7MINIV3/target.h @@ -88,6 +88,13 @@ #define MPU6000_SPI_BUS BUS_SPI1 #define MPU6000_CS_PIN PB2 +// ICM42688 +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PB2 +#define ICM42605_EXTI_PIN PA4 + /* * Blackbox Onboard Flash */ From 2eac713aa4384c32d9574a9bb20fc80a783f04d3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 6 Dec 2023 00:06:45 +0100 Subject: [PATCH 175/199] Add VTX power info --- docs/boards/MAMBAH743_2022B.md | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 docs/boards/MAMBAH743_2022B.md diff --git a/docs/boards/MAMBAH743_2022B.md b/docs/boards/MAMBAH743_2022B.md new file mode 100644 index 00000000000..688c630cb84 --- /dev/null +++ b/docs/boards/MAMBAH743_2022B.md @@ -0,0 +1,5 @@ +# VTX Power SWITCH + +Contrary to what the documentation suggests, VTX power is actually on USER2. + + From d1ac51fa1a0fd598f10a6dd9b23f943685bc78c9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 6 Dec 2023 13:57:28 +0100 Subject: [PATCH 176/199] Kakute F4 V2.4 --- src/main/target/KAKUTEF4/CMakeLists.txt | 3 ++- src/main/target/KAKUTEF4/target.c | 12 ++++++------ src/main/target/KAKUTEF4/target.h | 18 +++++++++++------- 3 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/main/target/KAKUTEF4/CMakeLists.txt b/src/main/target/KAKUTEF4/CMakeLists.txt index ae1ba2a46a2..caf084129b2 100644 --- a/src/main/target/KAKUTEF4/CMakeLists.txt +++ b/src/main/target/KAKUTEF4/CMakeLists.txt @@ -1,3 +1,4 @@ target_stm32f405xg(KAKUTEF4) target_stm32f405xg(KAKUTEF4V2) -target_stm32f405xg(KAKUTEF4V23) \ No newline at end of file +target_stm32f405xg(KAKUTEF4V23) +target_stm32f405xg(KAKUTEF4V24) \ No newline at end of file diff --git a/src/main/target/KAKUTEF4/target.c b/src/main/target/KAKUTEF4/target.c index ba47d09ffec..758dfa8b1bf 100755 --- a/src/main/target/KAKUTEF4/target.c +++ b/src/main/target/KAKUTEF4/target.c @@ -34,21 +34,21 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT - DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT - DMA1_ST2 DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT - DMA1_ST6 -#if !defined(KAKUTEF4V23) - DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT - DMA1_ST1 -#else - DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT - DMA1_ST1 +#if defined(KAKUTEF4V23) || defined(KAKUTEF4V24) + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT - DMA1_ST1 DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT - DMA1_ST0 DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT - DMA1_ST3 +#else + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT - DMA1_ST1 #endif -#if defined(KAKUTEF4V2) || defined(KAKUTEF4V23) +#if defined(KAKUTEF4V2) || defined(KAKUTEF4V23) || defined(KAKUTEF4V24) DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED_STRIP - DMA2_ST2 #else DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT - DMA1_ST2 DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S6_OUT - DMA2_ST4 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_LED, 0, 0), // LED_STRIP - DMA1_ST4 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_LED, 0, 0), // LED_STRIP - DMA1_ST4 #endif }; diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 1895b4b7e79..bc921c9f3f8 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -29,6 +29,9 @@ #elif defined(KAKUTEF4V23) # define TARGET_BOARD_IDENTIFIER "KT23" # define USBD_PRODUCT_STRING "KakuteF4-V2.3" +#elif defined(KAKUTEF4V24) +# define TARGET_BOARD_IDENTIFIER "KT24" +# define USBD_PRODUCT_STRING "KakuteF4-V2.4" #else # define TARGET_BOARD_IDENTIFIER "KTV1" # define USBD_PRODUCT_STRING "KakuteF4-V1" @@ -37,7 +40,7 @@ #define LED0 PB5 #define LED1 PB4 -#if !defined(KAKUTEF4V23) +#if defined(KAKUTEF4) || defined(KAKUTEF4V2) # define LED2 PB6 #endif @@ -54,7 +57,7 @@ #define MPU6000_CS_PIN PC4 #define MPU6000_SPI_BUS BUS_SPI1 -#if defined(KAKUTEF4V2) || defined(KAKUTEF4V23) +#if defined(KAKUTEF4V2) || defined(KAKUTEF4V23) || defined(KAKUTEF4V24) # define USE_I2C # define USE_I2C_DEVICE_1 # define I2C1_SCL PB8 // SCL pad @@ -79,11 +82,12 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PB14 -#define M25P16_CS_PIN PB3 -#define M25P16_SPI_BUS BUS_SPI3 - #define USE_FLASHFS + #define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_BUS BUS_SPI3 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define USB_IO #define USE_VCP @@ -105,7 +109,7 @@ #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 -#if defined(KAKUTEF4V2) || defined(KAKUTEF4V23) +#if defined(KAKUTEF4V2) || defined(KAKUTEF4V23) || defined(KAKUTEF4V24) # define USE_UART4 # define UART4_RX_PIN PA1 # define UART4_TX_PIN PA0 @@ -153,7 +157,7 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_BLACKBOX) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART3 From 38404dce525a2b5941b73299279bb5299938b97f Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 6 Dec 2023 18:11:18 +0100 Subject: [PATCH 177/199] Skip release for all F411 boards --- src/main/target/FLYWOOF411/CMakeLists.txt | 4 ++-- src/main/target/HAKRCF411D/CMakeLists.txt | 2 +- src/main/target/IFLIGHTF4_SUCCEXD/CMakeLists.txt | 2 +- src/main/target/MATEKF411/CMakeLists.txt | 8 ++++---- src/main/target/MATEKF411SE/CMakeLists.txt | 8 ++++---- src/main/target/MATEKF411TE/CMakeLists.txt | 2 +- src/main/target/NOX/CMakeLists.txt | 2 +- 7 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/target/FLYWOOF411/CMakeLists.txt b/src/main/target/FLYWOOF411/CMakeLists.txt index bf329e0d5fb..57d968a2bb6 100644 --- a/src/main/target/FLYWOOF411/CMakeLists.txt +++ b/src/main/target/FLYWOOF411/CMakeLists.txt @@ -1,2 +1,2 @@ -target_stm32f411xe(FLYWOOF411) -target_stm32f411xe(FLYWOOF411_V2) +target_stm32f411xe(FLYWOOF411 SKIP_RELEASES) +target_stm32f411xe(FLYWOOF411_V2 SKIP_RELEASES) diff --git a/src/main/target/HAKRCF411D/CMakeLists.txt b/src/main/target/HAKRCF411D/CMakeLists.txt index 4125ec3184f..c930981ec6f 100644 --- a/src/main/target/HAKRCF411D/CMakeLists.txt +++ b/src/main/target/HAKRCF411D/CMakeLists.txt @@ -1 +1 @@ -target_stm32f411xe(HAKRCF411D) +target_stm32f411xe(HAKRCF411D SKIP_RELEASES) diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/CMakeLists.txt b/src/main/target/IFLIGHTF4_SUCCEXD/CMakeLists.txt index 1c0c46b8955..09d7501e4d0 100644 --- a/src/main/target/IFLIGHTF4_SUCCEXD/CMakeLists.txt +++ b/src/main/target/IFLIGHTF4_SUCCEXD/CMakeLists.txt @@ -1 +1 @@ -target_stm32f411xe(IFLIGHTF4_SUCCEXD) +target_stm32f411xe(IFLIGHTF4_SUCCEXD SKIP_RELEASES) diff --git a/src/main/target/MATEKF411/CMakeLists.txt b/src/main/target/MATEKF411/CMakeLists.txt index 5907460815f..c582f2701bd 100644 --- a/src/main/target/MATEKF411/CMakeLists.txt +++ b/src/main/target/MATEKF411/CMakeLists.txt @@ -1,4 +1,4 @@ -target_stm32f411xe(MATEKF411) -target_stm32f411xe(MATEKF411_FD_SFTSRL) -target_stm32f411xe(MATEKF411_RSSI) -target_stm32f411xe(MATEKF411_SFTSRL2) +target_stm32f411xe(MATEKF411 SKIP_RELEASES) +target_stm32f411xe(MATEKF411_FD_SFTSRL SKIP_RELEASES) +target_stm32f411xe(MATEKF411_RSSI SKIP_RELEASES) +target_stm32f411xe(MATEKF411_SFTSRL2 SKIP_RELEASES) diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index c7b79ee6344..68b58ab583d 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1,4 +1,4 @@ -target_stm32f411xe(MATEKF411SE) -target_stm32f411xe(MATEKF411SE_PINIO) -target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) -target_stm32f411xe(MATEKF411SE_SS2_CH6) +target_stm32f411xe(MATEKF411SE SKIP_RELEASES) +target_stm32f411xe(MATEKF411SE_PINIO SKIP_RELEASES) +target_stm32f411xe(MATEKF411SE_FD_SFTSRL1 SKIP_RELEASES) +target_stm32f411xe(MATEKF411SE_SS2_CH6 SKIP_RELEASES) diff --git a/src/main/target/MATEKF411TE/CMakeLists.txt b/src/main/target/MATEKF411TE/CMakeLists.txt index 6ca2bd3f7b3..2b6a1a2af18 100644 --- a/src/main/target/MATEKF411TE/CMakeLists.txt +++ b/src/main/target/MATEKF411TE/CMakeLists.txt @@ -1 +1 @@ -target_stm32f411xe(MATEKF411TE) +target_stm32f411xe(MATEKF411TE SKIP_RELEASES) diff --git a/src/main/target/NOX/CMakeLists.txt b/src/main/target/NOX/CMakeLists.txt index b8189c76ec5..31d835ec865 100644 --- a/src/main/target/NOX/CMakeLists.txt +++ b/src/main/target/NOX/CMakeLists.txt @@ -1 +1 @@ -target_stm32f411xe(NOX) +target_stm32f411xe(NOX SKIP_RELEASES) From 440431982b7c6c10c29deefb88a5565fa80711d6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 6 Dec 2023 19:25:19 +0100 Subject: [PATCH 178/199] INAV 8 version bump --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index af42e31d7c1..db8ec36897b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,7 +51,7 @@ else() endif() endif() -project(INAV VERSION 7.0.0) +project(INAV VERSION 8.0.0) enable_language(ASM) From 91ca1047ed48fcac48c3a34d409a7d4cfc04ab9d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 7 Dec 2023 11:45:59 +0100 Subject: [PATCH 179/199] Add ICM42688P to KAKUTEH7 --- src/main/target/KAKUTEH7/target.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/target/KAKUTEH7/target.h b/src/main/target/KAKUTEH7/target.h index 8f88563c02b..258c7721aa0 100644 --- a/src/main/target/KAKUTEH7/target.h +++ b/src/main/target/KAKUTEH7/target.h @@ -113,6 +113,11 @@ #define IMU_BMI270_ALIGN CW0_DEG #endif +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_CS_PIN PE4 +#define ICM42605_SPI_BUS BUS_SPI4 + #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 From 9ba78683fc3152c8e247d9b7a535fc07a264ed69 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Thu, 7 Dec 2023 12:46:59 +0000 Subject: [PATCH 180/199] update f4 vcp in line with BF (#9544) --- .../Class/cdc/src/usbd_cdc_core.c | 193 ++++++++---------- src/main/vcpf4/usbd_cdc_vcp.c | 45 ++-- 2 files changed, 100 insertions(+), 138 deletions(-) diff --git a/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c b/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c index 2533cdb8774..a82ef08a2f0 100644 --- a/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c +++ b/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c @@ -184,8 +184,8 @@ __ALIGN_BEGIN uint8_t APP_Rx_Buffer [APP_RX_DATA_SIZE] __ALIGN_END ; #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ __ALIGN_BEGIN uint8_t CmdBuff[CDC_CMD_PACKET_SZE] __ALIGN_END ; -uint32_t APP_Rx_ptr_in = 0; -uint32_t APP_Rx_ptr_out = 0; +volatile uint32_t APP_Rx_ptr_in = 0; +volatile uint32_t APP_Rx_ptr_out = 0; uint32_t APP_Rx_length = 0; uint8_t USB_Tx_State = USB_CDC_IDLE; @@ -482,8 +482,8 @@ uint8_t usbd_cdc_Init (void *pdev, uint8_t usbd_cdc_DeInit (void *pdev, uint8_t cfgidx) { - (void)pdev; - (void)cfgidx; + (void)pdev; + (void)cfgidx; /* Open EP IN */ DCD_EP_Close(pdev, CDC_IN_EP); @@ -594,7 +594,7 @@ uint8_t usbd_cdc_Setup (void *pdev, */ uint8_t usbd_cdc_EP0_RxReady (void *pdev) { - (void)pdev; + (void)pdev; if (cdcCmd != NO_CMD) { /* Process the data */ @@ -617,60 +617,45 @@ uint8_t usbd_cdc_EP0_RxReady (void *pdev) */ uint8_t usbd_cdc_DataIn (void *pdev, uint8_t epnum) { - (void)pdev; - (void)epnum; - uint16_t USB_Tx_ptr; - uint16_t USB_Tx_length; - - if (USB_Tx_State == USB_CDC_BUSY) - { - if (APP_Rx_length == 0) - { - USB_Tx_State = USB_CDC_IDLE; - } - else + (void)pdev; + (void)epnum; + uint16_t USB_Tx_length; + + if (USB_Tx_State == USB_CDC_BUSY) { - if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE){ - USB_Tx_ptr = APP_Rx_ptr_out; - USB_Tx_length = CDC_DATA_IN_PACKET_SIZE; - - APP_Rx_ptr_out += CDC_DATA_IN_PACKET_SIZE; - APP_Rx_length -= CDC_DATA_IN_PACKET_SIZE; - } - else - { - USB_Tx_ptr = APP_Rx_ptr_out; - USB_Tx_length = APP_Rx_length; - - APP_Rx_ptr_out += APP_Rx_length; - APP_Rx_length = 0; - if(USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) + if (APP_Rx_length == 0) { - USB_Tx_State = USB_CDC_ZLP; + USB_Tx_State = USB_CDC_IDLE; + } else { + if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) { + USB_Tx_length = CDC_DATA_IN_PACKET_SIZE; + } else { + USB_Tx_length = APP_Rx_length; + + if (USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) { + USB_Tx_State = USB_CDC_ZLP; + } + } + + /* Prepare the available data buffer to be sent on IN endpoint */ + DCD_EP_Tx(pdev, CDC_IN_EP, (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out], USB_Tx_length); + + // Advance the out pointer + APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE; + APP_Rx_length -= USB_Tx_length; + + return USBD_OK; } - } - - /* Prepare the available data buffer to be sent on IN endpoint */ - DCD_EP_Tx (pdev, - CDC_IN_EP, - (uint8_t*)&APP_Rx_Buffer[USB_Tx_ptr], - USB_Tx_length); - return USBD_OK; } - } - - /* Avoid any asynchronous transfer during ZLP */ - if (USB_Tx_State == USB_CDC_ZLP) - { - /*Send ZLP to indicate the end of the current transfer */ - DCD_EP_Tx (pdev, - CDC_IN_EP, - NULL, - 0); - - USB_Tx_State = USB_CDC_IDLE; - } - return USBD_OK; + + /* Avoid any asynchronous transfer during ZLP */ + if (USB_Tx_State == USB_CDC_ZLP) { + /*Send ZLP to indicate the end of the current transfer */ + DCD_EP_Tx(pdev, CDC_IN_EP, NULL, 0); + + USB_Tx_State = USB_CDC_IDLE; + } + return USBD_OK; } /** @@ -731,67 +716,49 @@ uint8_t usbd_cdc_SOF (void *pdev) */ static void Handle_USBAsynchXfer (void *pdev) { - uint16_t USB_Tx_ptr; - uint16_t USB_Tx_length; - - if(USB_Tx_State == USB_CDC_IDLE) - { - if (APP_Rx_ptr_out == APP_RX_DATA_SIZE) - { - APP_Rx_ptr_out = 0; - } - - if(APP_Rx_ptr_out == APP_Rx_ptr_in) - { - USB_Tx_State = USB_CDC_IDLE; - return; - } - - if(APP_Rx_ptr_out > APP_Rx_ptr_in) /* rollback */ - { - APP_Rx_length = APP_RX_DATA_SIZE - APP_Rx_ptr_out; - - } - else - { - APP_Rx_length = APP_Rx_ptr_in - APP_Rx_ptr_out; - - } + uint16_t USB_Tx_length; + + if (USB_Tx_State == USB_CDC_IDLE) { + if (APP_Rx_ptr_out == APP_Rx_ptr_in) { + // Ring buffer is empty + return; + } + + if (APP_Rx_ptr_out > APP_Rx_ptr_in) { + // Transfer bytes up to the end of the ring buffer + APP_Rx_length = APP_RX_DATA_SIZE - APP_Rx_ptr_out; + } else { + // Transfer all bytes in ring buffer + APP_Rx_length = APP_Rx_ptr_in - APP_Rx_ptr_out; + } + #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED - APP_Rx_length &= ~0x03; + // Only transfer whole 32 bit words of data + APP_Rx_length &= ~0x03; #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ + + if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) { + USB_Tx_length = CDC_DATA_IN_PACKET_SIZE; + + USB_Tx_State = USB_CDC_BUSY; + } else { + USB_Tx_length = APP_Rx_length; + + if (USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) { + USB_Tx_State = USB_CDC_ZLP; + } else { + USB_Tx_State = USB_CDC_BUSY; + } + } - if (APP_Rx_length > CDC_DATA_IN_PACKET_SIZE) - { - USB_Tx_ptr = APP_Rx_ptr_out; - USB_Tx_length = CDC_DATA_IN_PACKET_SIZE; - - APP_Rx_ptr_out += CDC_DATA_IN_PACKET_SIZE; - APP_Rx_length -= CDC_DATA_IN_PACKET_SIZE; - USB_Tx_State = USB_CDC_BUSY; - } - else - { - USB_Tx_ptr = APP_Rx_ptr_out; - USB_Tx_length = APP_Rx_length; - - APP_Rx_ptr_out += APP_Rx_length; - APP_Rx_length = 0; - if(USB_Tx_length == CDC_DATA_IN_PACKET_SIZE) - { - USB_Tx_State = USB_CDC_ZLP; - } - else - { - USB_Tx_State = USB_CDC_BUSY; - } + DCD_EP_Tx (pdev, + CDC_IN_EP, + (uint8_t*)&APP_Rx_Buffer[APP_Rx_ptr_out], + USB_Tx_length); + + APP_Rx_ptr_out = (APP_Rx_ptr_out + USB_Tx_length) % APP_RX_DATA_SIZE; + APP_Rx_length -= USB_Tx_length; } - - DCD_EP_Tx (pdev, - CDC_IN_EP, - (uint8_t*)&APP_Rx_Buffer[USB_Tx_ptr], - USB_Tx_length); - } } /** @@ -803,7 +770,7 @@ static void Handle_USBAsynchXfer (void *pdev) */ static uint8_t *USBD_cdc_GetCfgDesc (uint8_t speed, uint16_t *length) { - (void)speed; + (void)speed; *length = sizeof (usbd_cdc_CfgDesc); return usbd_cdc_CfgDesc; } diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 2fc2da8b1de..15c1f7e39d9 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -19,16 +19,18 @@ ****************************************************************************** */ -#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED -#pragma data_alignment = 4 -#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ - /* Includes ------------------------------------------------------------------*/ #include "usbd_cdc_vcp.h" #include "stm32f4xx_conf.h" -#include "stdbool.h" +#include #include "drivers/time.h" +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED +#pragma data_alignment = 4 +#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ + +__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; + LINE_CODING g_lc; extern __IO uint8_t USB_Tx_State; @@ -38,11 +40,11 @@ __IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ /* This is the buffer for data received from the MCU to APP (i.e. MCU TX, APP RX) */ extern uint8_t APP_Rx_Buffer[]; -extern uint32_t APP_Rx_ptr_out; +extern volatile uint32_t APP_Rx_ptr_out; /* Increment this buffer position or roll it back to start address when writing received data in the buffer APP_Rx_Buffer. */ -extern uint32_t APP_Rx_ptr_in; +extern volatile uint32_t APP_Rx_ptr_in; /* APP TX is the circular buffer for data that is transmitted from the APP (host) @@ -63,7 +65,6 @@ static void *ctrlLineStateCbContext; static void (*baudRateCb)(void *context, uint32_t baud); static void *baudRateCbContext; -__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; CDC_IF_Prop_TypeDef VCP_fops = {VCP_Init, VCP_DeInit, VCP_Ctrl, VCP_DataTx, VCP_DataRx }; @@ -132,7 +133,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) //Note - hw flow control on UART 1-3 and 6 only case SET_LINE_CODING: // If a callback is provided, tell the upper driver of changes in baud rate - if (plc && (Len == sizeof (*plc))) { + if (plc && (Len == sizeof(*plc))) { if (baudRateCb) { baudRateCb(baudRateCbContext, plc->bitrate); } @@ -142,7 +143,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) case GET_LINE_CODING: - if (plc && (Len == sizeof (*plc))) { + if (plc && (Len == sizeof(*plc))) { ust_cpy(plc, &g_lc); } break; @@ -150,7 +151,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) case SET_CONTROL_LINE_STATE: // If a callback is provided, tell the upper driver of changes in DTR/RTS state - if (plc && (Len == sizeof (uint16_t))) { + if (plc && (Len == sizeof(uint16_t))) { if (ctrlLineStateCb) { ctrlLineStateCb(ctrlLineStateCbContext, *((uint16_t *)Buf)); } @@ -183,14 +184,7 @@ uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength) uint32_t CDC_Send_FreeBytes(void) { - /* - return the bytes free in the circular buffer - - functionally equivalent to: - (APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE - APP_Rx_ptr_in + APP_Rx_ptr_in) - but without the impact of the condition check. - */ - return ((APP_Rx_ptr_out - APP_Rx_ptr_in) + (-((int)(APP_Rx_ptr_out <= APP_Rx_ptr_in)) & APP_RX_DATA_SIZE)) - 1; + return APP_RX_DATA_SIZE - CDC_Receive_BytesAvailable(); } /** @@ -210,12 +204,13 @@ static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) while (USB_Tx_State != 0); for (uint32_t i = 0; i < Len; i++) { - APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; - APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; - - while (CDC_Send_FreeBytes() == 0) { + // Stall if the ring buffer is full + while (((APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE) == APP_Rx_ptr_out) { delay(1); } + + APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; + APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; } return USBD_OK; @@ -232,7 +227,7 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) { uint32_t count = 0; - while (APP_Tx_ptr_out != APP_Tx_ptr_in && count < len) { + while (APP_Tx_ptr_out != APP_Tx_ptr_in && (count < len)) { recvBuf[count] = APP_Tx_Buffer[APP_Tx_ptr_out]; APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % APP_TX_DATA_SIZE; count++; @@ -243,7 +238,7 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) uint32_t CDC_Receive_BytesAvailable(void) { /* return the bytes available in the receive circular buffer */ - return APP_Tx_ptr_out > APP_Tx_ptr_in ? APP_TX_DATA_SIZE - APP_Tx_ptr_out + APP_Tx_ptr_in : APP_Tx_ptr_in - APP_Tx_ptr_out; + return (APP_Tx_ptr_in + APP_TX_DATA_SIZE - APP_Tx_ptr_out) % APP_TX_DATA_SIZE; } /** From 4e705ac2093316105a65fd016204b6b6beb24eae Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 8 Dec 2023 10:03:44 +0100 Subject: [PATCH 181/199] INAV 7.1 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index af42e31d7c1..d8247e7cb5a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,7 +51,7 @@ else() endif() endif() -project(INAV VERSION 7.0.0) +project(INAV VERSION 7.1.0) enable_language(ASM) From 123caeed4d42e3786236f6254cecca4de2e1d6bf Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 8 Dec 2023 17:40:39 +0000 Subject: [PATCH 182/199] Update navigation.c --- src/main/navigation/navigation.c | 63 ++++++++++++++++---------------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d217d79055c..c53962b2ea3 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -132,7 +132,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, #endif .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h) - .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s) + .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s) .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, @@ -1450,7 +1450,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); posControl.landingDelay = 0; - + if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) posControl.rthState.rthLinearDescentActive = false; @@ -2535,10 +2535,10 @@ bool validateRTHSanityChecker(void) #ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { //disable sanity checks in GPS estimation mode - //when estimated GPS fix is replaced with real fix, coordinates may jump + //when estimated GPS fix is replaced with real fix, coordinates may jump posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump - posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; + posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; return true; } #endif @@ -3851,7 +3851,6 @@ void checkManualEmergencyLandingControl(bool forcedActivation) static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) { - static bool canActivateWaypoint = false; static bool canActivateLaunchMode = false; //We can switch modes only when ARMED @@ -3899,10 +3898,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } posControl.rthSanityChecker.rthSanityOK = true; - // Keep canActivateWaypoint flag at FALSE if there is no mission loaded - // Also block WP mission if we are executing RTH - if (!isWaypointMissionValid() || isExecutingRTH) { + /* WP mission activation control: + * canActivateWaypoint & waypointWasActivated are used to prevent WP mission + * auto restarting after interruption by Manual or RTH modes. + * WP mode must be deselected before it can be reactivated again. */ + static bool waypointWasActivated = false; + const bool isWpMissionLoaded = isWaypointMissionValid(); + bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner + + if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { canActivateWaypoint = false; + if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { + canActivateWaypoint = true; + waypointWasActivated = false; + } } /* Airplane specific modes */ @@ -3938,22 +3947,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } } - // Failsafe_RTH (can override MANUAL) + /* If we request forced RTH - attempt to activate it no matter what + * This might switch to emergency landing controller if GPS is unavailable */ if (posControl.flags.forcedRTHActivated) { - // If we request forced RTH - attempt to activate it no matter what - // This might switch to emergency landing controller if GPS is unavailable return NAV_FSM_EVENT_SWITCH_TO_RTH; } - /* Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded - * Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection) - * Also prevent WP falling back to RTH if WP mission planner is active */ - const bool blockWPFallback = IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive; - if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !blockWPFallback)) { + /* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded. + * WP prevented from falling back to RTH if WP mission planner is active */ + const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; + if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) { // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss - // If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold + // Without this loss of any of the canActivateNavigation && canActivateAltHold // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back - // logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc) + // logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { return NAV_FSM_EVENT_SWITCH_TO_RTH; } @@ -3961,24 +3968,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // MANUAL mode has priority over WP/PH/AH if (IS_RC_MODE_ACTIVE(BOXMANUAL)) { - canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode return NAV_FSM_EVENT_SWITCH_TO_IDLE; } - // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded - // Block activation if using WP Mission Planner - // Also check multimission mission change status before activating WP mode + // Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded. + // Also check multimission mission change status before activating WP mode. #ifdef USE_MULTI_MISSION - if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { + if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { #else - if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { + if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { #endif - if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) + if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { + waypointWasActivated = true; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; - } - else { - // Arm the state variable if the WP BOX mode is not enabled - canActivateWaypoint = true; + } } if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) { @@ -4009,8 +4012,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD; } } else { - canActivateWaypoint = false; - // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight) canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid())); } From 879b20d5186bf80507bde06fbb9dd0eebe4b8581 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 8 Dec 2023 17:40:39 +0000 Subject: [PATCH 183/199] Update navigation.c --- src/main/navigation/navigation.c | 59 ++++++++++++++++---------------- 1 file changed, 30 insertions(+), 29 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0a3cfb91b8f..8e210aa533e 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -132,7 +132,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, #endif .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h) - .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s) + .min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s) .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, @@ -1450,7 +1450,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); posControl.landingDelay = 0; - + if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) posControl.rthState.rthLinearDescentActive = false; @@ -3840,7 +3840,6 @@ void checkManualEmergencyLandingControl(bool forcedActivation) static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) { - static bool canActivateWaypoint = false; static bool canActivateLaunchMode = false; //We can switch modes only when ARMED @@ -3888,10 +3887,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } posControl.rthSanityChecker.rthSanityOK = true; - // Keep canActivateWaypoint flag at FALSE if there is no mission loaded - // Also block WP mission if we are executing RTH - if (!isWaypointMissionValid() || isExecutingRTH) { + /* WP mission activation control: + * canActivateWaypoint & waypointWasActivated are used to prevent WP mission + * auto restarting after interruption by Manual or RTH modes. + * WP mode must be deselected before it can be reactivated again. */ + static bool waypointWasActivated = false; + const bool isWpMissionLoaded = isWaypointMissionValid(); + bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner + + if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) { canActivateWaypoint = false; + if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { + canActivateWaypoint = true; + waypointWasActivated = false; + } } /* Airplane specific modes */ @@ -3927,22 +3936,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } } - // Failsafe_RTH (can override MANUAL) + /* If we request forced RTH - attempt to activate it no matter what + * This might switch to emergency landing controller if GPS is unavailable */ if (posControl.flags.forcedRTHActivated) { - // If we request forced RTH - attempt to activate it no matter what - // This might switch to emergency landing controller if GPS is unavailable return NAV_FSM_EVENT_SWITCH_TO_RTH; } - /* Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded - * Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection) - * Also prevent WP falling back to RTH if WP mission planner is active */ - const bool blockWPFallback = IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive; - if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !blockWPFallback)) { + /* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded. + * WP prevented from falling back to RTH if WP mission planner is active */ + const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; + if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) { // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss - // If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold + // Without this loss of any of the canActivateNavigation && canActivateAltHold // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back - // logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc) + // logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { return NAV_FSM_EVENT_SWITCH_TO_RTH; } @@ -3950,24 +3957,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // MANUAL mode has priority over WP/PH/AH if (IS_RC_MODE_ACTIVE(BOXMANUAL)) { - canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode return NAV_FSM_EVENT_SWITCH_TO_IDLE; } - // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded - // Block activation if using WP Mission Planner - // Also check multimission mission change status before activating WP mode + // Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded. + // Also check multimission mission change status before activating WP mode. #ifdef USE_MULTI_MISSION - if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { + if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { #else - if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { + if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) { #endif - if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) + if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { + waypointWasActivated = true; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; - } - else { - // Arm the state variable if the WP BOX mode is not enabled - canActivateWaypoint = true; + } } if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) { @@ -3998,8 +4001,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD; } } else { - canActivateWaypoint = false; - // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight) canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid())); } From 5d4cb57b13774a878090828d04b859275d181cdb Mon Sep 17 00:00:00 2001 From: Julio Cesar Date: Thu, 14 Dec 2023 22:38:59 -0300 Subject: [PATCH 184/199] fix default param name --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 17384f92732..4ddf5cc1f25 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4828,7 +4828,7 @@ Character to use for OSD switch incicator 3. | Default | Min | Max | | --- | --- | --- | -| LIGT | | 5 | +| LIGHT | | 5 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1011638959d..5bb06bed96e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3613,7 +3613,7 @@ groups: field: osd_switch_indicator3_name type: string max: 5 - default_value: "LIGT" + default_value: "LIGHT" - name: osd_switch_indicator_zero_channel description: "RC Channel to use for OSD switch indicator 0." From 672aa01f607fb7b72f27c58a863fb7a93ff88576 Mon Sep 17 00:00:00 2001 From: Julio Cesar Date: Thu, 14 Dec 2023 22:55:09 -0300 Subject: [PATCH 185/199] fix telemetry name --- docs/SITL/SITL.md | 2 +- docs/Settings.md | 2 +- lib/main/MAVLink/common/common.h | 2 +- src/main/fc/settings.yaml | 2 +- src/main/io/osd.c | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/SITL/SITL.md b/docs/SITL/SITL.md index 8dca15e22f3..0c127135781 100644 --- a/docs/SITL/SITL.md +++ b/docs/SITL/SITL.md @@ -79,7 +79,7 @@ For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/) For SBUS, the command line arguments of the python script are: ```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000``` -### Telemtry +### Telemetry LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP. diff --git a/docs/Settings.md b/docs/Settings.md index 4ddf5cc1f25..17384f92732 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4828,7 +4828,7 @@ Character to use for OSD switch incicator 3. | Default | Min | Max | | --- | --- | --- | -| LIGHT | | 5 | +| LIGT | | 5 | --- diff --git a/lib/main/MAVLink/common/common.h b/lib/main/MAVLink/common/common.h index 55200c5a84b..ec5fc7218d2 100755 --- a/lib/main/MAVLink/common/common.h +++ b/lib/main/MAVLink/common/common.h @@ -48,7 +48,7 @@ typedef enum FIRMWARE_VERSION_TYPE } FIRMWARE_VERSION_TYPE; #endif -/** @brief Flags to report failure cases over the high latency telemtry. */ +/** @brief Flags to report failure cases over the high latency telemetry. */ #ifndef HAVE_ENUM_HL_FAILURE_FLAG #define HAVE_ENUM_HL_FAILURE_FLAG typedef enum HL_FAILURE_FLAG diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5bb06bed96e..1011638959d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3613,7 +3613,7 @@ groups: field: osd_switch_indicator3_name type: string max: 5 - default_value: "LIGHT" + default_value: "LIGT" - name: osd_switch_indicator_zero_channel description: "RC Channel to use for OSD switch indicator 0." diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1662094cabb..17db9297e49 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3685,7 +3685,7 @@ void osdDrawNextElement(void) elementIndex = osdIncElementIndex(elementIndex); } while (!osdDrawSingleElement(elementIndex) && index != elementIndex); - // Draw artificial horizon + tracking telemtry last + // Draw artificial horizon + tracking telemetry last osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); if (osdConfig()->telemetry>0){ osdDisplayTelemetry(); From 6a58f69b6bea384e42c567822088b6238c922f5c Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 15 Dec 2023 16:48:09 +0100 Subject: [PATCH 186/199] allow simulating airspeed sensor in SITL with HITL plugin --- src/main/sensors/pitotmeter.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 13b9fb54255..baf7bed0a61 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -275,15 +275,15 @@ STATIC_PROTOTHREAD(pitotThread) pitot.airSpeed = 0.0f; } -#ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitot.airSpeed = simulatorData.airSpeed; - } -#endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitot.airSpeed = fakePitotGetAirspeed(); } +#endif +#ifdef USE_SIMULATOR + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + pitot.airSpeed = simulatorData.airSpeed; + } #endif } From 0535617286bda0313364386a01a23f26d268623a Mon Sep 17 00:00:00 2001 From: Martin Luessi Date: Sat, 16 Dec 2023 08:23:14 -0800 Subject: [PATCH 187/199] USB MSC: Fix SCIS mode sense write protection bit (#9572) --- .../Class/MSC/Src/usbd_msc_scsi.c | 12 ++++++++++++ .../Class/MSC/Src/usbd_msc_scsi.c | 12 ++++++++++++ .../Class/MSC/Src/usbd_msc_scsi.c | 11 +++++++++++ 3 files changed, 35 insertions(+) diff --git a/lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c b/lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c index 5d4d2521693..3c63a978f2c 100644 --- a/lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c +++ b/lib/main/STM32F4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c @@ -343,6 +343,12 @@ static int8_t SCSI_ModeSense6 (USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t * len--; hmsc->bot_data[len] = MSC_Mode_Sense6_data[len]; } + + // set bit 7 of the device configuration byte to indicate write protection + if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) { + hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7); + } + return 0; } @@ -365,6 +371,12 @@ static int8_t SCSI_ModeSense10 (USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t len--; hmsc->bot_data[len] = MSC_Mode_Sense10_data[len]; } + + // set bit 7 of the device configuration byte to indicate write protection + if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) { + hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7); + } + return 0; } diff --git a/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c b/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c index ed3deef5959..b1d76cc25da 100644 --- a/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c +++ b/lib/main/STM32F7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c @@ -347,6 +347,12 @@ static int8_t SCSI_ModeSense6 (USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t * len--; hmsc->bot_data[len] = MSC_Mode_Sense6_data[len]; } + + // set bit 7 of the device configuration byte to indicate write protection + if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) { + hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7); + } + return 0; } @@ -370,6 +376,12 @@ static int8_t SCSI_ModeSense10 (USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t len--; hmsc->bot_data[len] = MSC_Mode_Sense10_data[len]; } + + // set bit 7 of the device configuration byte to indicate write protection + if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) { + hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7); + } + return 0; } diff --git a/lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c b/lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c index d15d9039a54..ffc615dac5a 100644 --- a/lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c +++ b/lib/main/STM32H7/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc_scsi.c @@ -346,6 +346,12 @@ static int8_t SCSI_ModeSense6(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t *p len--; hmsc->bot_data[len] = MSC_Mode_Sense6_data[len]; } + + // set bit 7 of the device configuration byte to indicate write protection + if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) { + hmsc->bot_data[2] = hmsc->bot_data[2] | (1 << 7); + } + return 0; } @@ -371,6 +377,11 @@ static int8_t SCSI_ModeSense10(USBD_HandleTypeDef *pdev, uint8_t lun, uint8_t * hmsc->bot_data[len] = MSC_Mode_Sense10_data[len]; } + // set bit 7 of the device configuration byte to indicate write protection + if (((USBD_StorageTypeDef *)pdev->pMSC_UserData)->IsWriteProtected(lun) != 0) { + hmsc->bot_data[3] = hmsc->bot_data[3] | (1 << 7); + } + return 0; } From 874407ba61060fa330eb56b30399697dfe5e4ccf Mon Sep 17 00:00:00 2001 From: error414 Date: Mon, 9 Oct 2023 19:18:22 +0200 Subject: [PATCH 188/199] ADSB support for inav over mavlink --- .gitignore | 1 + docs/ADSB.md | 17 +++ docs/Settings.md | 30 ++++ src/main/CMakeLists.txt | 1 + src/main/drivers/osd_symbols.h | 2 + src/main/fc/fc_msp.c | 35 +++++ src/main/fc/fc_tasks.c | 21 +++ src/main/fc/settings.yaml | 25 ++++ src/main/io/adsb.c | 223 ++++++++++++++++++++++++++++ src/main/io/adsb.h | 67 +++++++++ src/main/io/osd.c | 74 +++++++++ src/main/io/osd.h | 7 + src/main/msp/msp_protocol_v2_inav.h | 2 + src/main/scheduler/scheduler.h | 3 + src/main/target/common.h | 8 + src/main/telemetry/mavlink.c | 49 ++++++ 16 files changed, 565 insertions(+) create mode 100644 docs/ADSB.md create mode 100644 src/main/io/adsb.c create mode 100644 src/main/io/adsb.h diff --git a/.gitignore b/.gitignore index b2c3b9ff546..87ca5a87c7b 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,4 @@ make/local.mk launch.json .vscode/tasks.json .vscode/c_cpp_properties.json +/cmake-build-debug/ diff --git a/docs/ADSB.md b/docs/ADSB.md new file mode 100644 index 00000000000..345af30a97e --- /dev/null +++ b/docs/ADSB.md @@ -0,0 +1,17 @@ +# ADS-B + +[Automatic Dependent Surveillance Broadcast](https://en.wikipedia.org/wiki/Automatic_Dependent_Surveillance%E2%80%93Broadcast) +is an air traffic surveillance technology that enables aircraft to be accurately tracked by air traffic controllers and other pilots without the need for conventional radar. + +## Current state + +OSD can be configured to shows the closest aircraft. + +## Hardware + +All ADSB receivers which can send Mavlink [ADSB_VEHICLE](https://mavlink.io/en/messages/common.html#ADSB_VEHICLE) message are supported + +* [PINGRX](https://uavionix.com/product/pingrx-pro/) (not tested) +* [TT-SC1](https://www.aerobits.pl/product/aero/) (tested) + + diff --git a/docs/Settings.md b/docs/Settings.md index adc17297c1e..1872bf84a17 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4002,6 +4002,36 @@ _// TODO_ --- +### osd_adsb_distance_alert + +Distance inside which ADSB data flashes for proximity warning + +| Default | Min | Max | +| --- | --- | --- | +| 3000 | 1 | 64000 | + +--- + +### osd_adsb_distance_warning + +Distance in meters of ADSB aircraft that is displayed + +| Default | Min | Max | +| --- | --- | --- | +| 20000 | 1 | 64000 | + +--- + +### osd_adsb_ignore_plane_above_me_limit + +Ignore adsb planes above, limit, 0 disabled (meters) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 64000 | + +--- + ### osd_ahi_bordered Shows a border/corners around the AHI region (pixel OSD only) diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 13ac4a92911..1140cdd5080 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -344,6 +344,7 @@ main_sources(COMMON_SRC flight/ez_tune.c flight/ez_tune.h + io/adsb.c io/beeper.c io/beeper.h io/servo_sbus.c diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index d645184e5b4..2437b9d3c2c 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -180,6 +180,8 @@ #define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error +#define SYM_ADSB 0xFD // 253 ADSB + #define SYM_LOGO_START 0x101 // 257 to 297, INAV logo #define SYM_LOGO_WIDTH 10 #define SYM_LOGO_HEIGHT 4 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 952f13dddb0..8de7ca1c0d0 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -83,6 +83,7 @@ #include "config/config_eeprom.h" #include "config/feature.h" +#include "io/adsb.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/flashfs.h" #include "io/gps.h" @@ -948,6 +949,33 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, gpsSol.epv); break; #endif + case MSP2_ADSB_VEHICLE_LIST: +#ifdef USE_ADSB + sbufWriteU8(dst, MAX_ADSB_VEHICLES); + sbufWriteU8(dst, ADSB_CALL_SIGN_MAX_LENGTH); + + for(uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++){ + + adsbVehicle_t *adsbVehicle = findVehicle(i); + + for(uint8_t ii = 0; ii < ADSB_CALL_SIGN_MAX_LENGTH; ii++){ + sbufWriteU8(dst, adsbVehicle->vehicleValues.callsign[ii]); + } + + sbufWriteU32(dst, adsbVehicle->vehicleValues.icao); + sbufWriteU32(dst, adsbVehicle->vehicleValues.lat); + sbufWriteU32(dst, adsbVehicle->vehicleValues.lon); + sbufWriteU32(dst, adsbVehicle->vehicleValues.alt); + sbufWriteU16(dst, (uint16_t)CENTIDEGREES_TO_DEGREES(adsbVehicle->vehicleValues.heading)); + sbufWriteU8(dst, adsbVehicle->vehicleValues.tslc); + sbufWriteU8(dst, adsbVehicle->vehicleValues.emitterType); + sbufWriteU8(dst, adsbVehicle->ttl); + } +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif + break; case MSP_DEBUG: // output some useful QA statistics // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] @@ -1518,6 +1546,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #else sbufWriteU16(dst, 0); sbufWriteU16(dst, 0); +#endif +#ifdef USE_ADSB + sbufWriteU16(dst, osdConfig()->adsb_distance_warning); + sbufWriteU16(dst, osdConfig()->adsb_distance_alert); +#else + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); #endif break; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 607a814bed5..246d65c0a7e 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -69,6 +69,7 @@ #include "io/osd_dji_hd.h" #include "io/displayport_msp_osd.h" #include "io/servo_sbus.h" +#include "io/adsb.h" #include "msp/msp_serial.h" @@ -181,6 +182,14 @@ void taskUpdateCompass(timeUs_t currentTimeUs) } #endif +#ifdef USE_ADSB +void taskAdsb(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + adsbTtlClean(currentTimeUs); +} +#endif + #ifdef USE_BARO void taskUpdateBaro(timeUs_t currentTimeUs) { @@ -360,6 +369,9 @@ void fcTasksInit(void) #ifdef USE_PITOT setTaskEnabled(TASK_PITOT, sensors(SENSOR_PITOT)); #endif +#ifdef USE_ADSB + setTaskEnabled(TASK_ADSB, true); +#endif #ifdef USE_RANGEFINDER setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER)); #endif @@ -495,6 +507,15 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#ifdef USE_ADSB + [TASK_ADSB] = { + .taskName = "ADSB", + .taskFunc = taskAdsb, + .desiredPeriod = TASK_PERIOD_HZ(1), // ADSB is updated at 1 Hz + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif + #ifdef USE_BARO [TASK_BARO] = { .taskName = "BARO", diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 09823394935..963783ed946 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3444,6 +3444,31 @@ groups: max: 11 default_value: 9 + - name: osd_adsb_distance_warning + description: "Distance in meters of ADSB aircraft that is displayed" + default_value: 20000 + condition: USE_ADSB + field: adsb_distance_warning + min: 1 + max: 64000 + type: uint16_t + - name: osd_adsb_distance_alert + description: "Distance inside which ADSB data flashes for proximity warning" + default_value: 3000 + condition: USE_ADSB + field: adsb_distance_alert + min: 1 + max: 64000 + type: uint16_t + - name: osd_adsb_ignore_plane_above_me_limit + description: "Ignore adsb planes above, limit, 0 disabled (meters)" + default_value: 0 + condition: USE_ADSB + field: adsb_ignore_plane_above_me_limit + min: 0 + max: 64000 + type: uint16_t + - name: osd_estimations_wind_compensation description: "Use wind estimation for remaining flight time/distance estimation" default_value: ON diff --git a/src/main/io/adsb.c b/src/main/io/adsb.c new file mode 100644 index 00000000000..573112c2df7 --- /dev/null +++ b/src/main/io/adsb.c @@ -0,0 +1,223 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + +#include + + +#include "adsb.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "common/maths.h" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#include "common/mavlink.h" +#pragma GCC diagnostic pop + + +#include "math.h" + + +#ifdef USE_ADSB + +adsbVehicle_t adsbVehiclesList[MAX_ADSB_VEHICLES]; +adsbVehicleStatus_t adsbVehiclesStatus; + +adsbVehicleValues_t vehicleValues; + + +adsbVehicleValues_t* getVehicleForFill(void){ + return &vehicleValues; +} + +// use bsearch function +adsbVehicle_t *findVehicleByIcao(uint32_t avicao) { + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (avicao == adsbVehiclesList[i].vehicleValues.icao) { + return &adsbVehiclesList[i]; + } + } + return NULL; +} + +adsbVehicle_t *findVehicleFarthest(void) { + adsbVehicle_t *adsbLocal = NULL; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist < adsbVehiclesList[i].calculatedVehicleValues.dist)) { + adsbLocal = &adsbVehiclesList[i]; + } + } + return adsbLocal; +} + +uint8_t getActiveVehiclesCount(void) { + uint8_t total = 0; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl > 0) { + total++; + } + } + return total; +} + +adsbVehicle_t *findVehicleClosest(void) { + adsbVehicle_t *adsbLocal = NULL; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist > adsbVehiclesList[i].calculatedVehicleValues.dist)) { + adsbLocal = &adsbVehiclesList[i]; + } + } + return adsbLocal; +} + +adsbVehicle_t *findFreeSpaceInList(void) { + //find expired first + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl == 0) { + return &adsbVehiclesList[i]; + } + } + + return NULL; +} + +adsbVehicle_t *findVehicleNotCalculated(void) { + //find expired first + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].calculatedVehicleValues.valid == false) { + return &adsbVehiclesList[i]; + } + } + + return NULL; +} + +adsbVehicle_t* findVehicle(uint8_t index) +{ + if (index < MAX_ADSB_VEHICLES){ + return &adsbVehiclesList[index]; + } + + return NULL; +} + +adsbVehicleStatus_t* getAdsbStatus(void){ + return &adsbVehiclesStatus; +} + +void gpsDistanceCmBearing(int32_t currentLat1, int32_t currentLon1, int32_t destinationLat2, int32_t destinationLon2, uint32_t *dist, int32_t *bearing) { + float GPS_scaleLonDown = cos_approx((fabsf((float) gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f); + const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees + const float dLon = (float) (destinationLon2 - currentLon1) * GPS_scaleLonDown; + + *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; + *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg + *bearing = wrap_36000(*bearing); +}; + +void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) { + + // no valid lat lon or altitude + if((vehicleValuesLocal->flags & (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)) != (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)){ + return; + } + + adsbVehiclesStatus.vehiclesMessagesTotal++; + adsbVehicle_t *vehicle = NULL; + + vehicle = findVehicleByIcao(vehicleValuesLocal->icao); + if(vehicle != NULL && vehicleValuesLocal->tslc > ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST){ + vehicle->ttl = 0; + return; + } + + // non GPS mode, GPS is not fix, just find free space in list or by icao and save vehicle without calculated values + if (!enviromentOkForCalculatingDistaceBearing()) { + + if(vehicle == NULL){ + vehicle = findFreeSpaceInList(); + } + + if (vehicle != NULL) { + memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues)); + vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST; + vehicle->calculatedVehicleValues.valid = false; + return; + } + } else { + // GPS mode, GPS is fixed and has enough sats + + + if(vehicle == NULL){ + vehicle = findFreeSpaceInList(); + } + + if(vehicle == NULL){ + vehicle = findVehicleNotCalculated(); + } + + if(vehicle == NULL){ + vehicle = findVehicleFarthest(); + } + + if (vehicle != NULL) { + memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues)); + recalculateVehicle(vehicle); + vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST; + return; + } + } +}; + +void recalculateVehicle(adsbVehicle_t* vehicle){ + gpsDistanceCmBearing(gpsSol.llh.lat, gpsSol.llh.lon, vehicle->vehicleValues.lat, vehicle->vehicleValues.lon, &(vehicle->calculatedVehicleValues.dist), &(vehicle->calculatedVehicleValues.dir)); + + if (vehicle != NULL && vehicle->calculatedVehicleValues.dist > ADSB_LIMIT_CM) { + vehicle->ttl = 0; + return; + } + + vehicle->calculatedVehicleValues.verticalDistance = vehicle->vehicleValues.alt - (int32_t)getEstimatedActualPosition(Z) - GPS_home.alt; + vehicle->calculatedVehicleValues.valid = true; +} + +void adsbTtlClean(timeUs_t currentTimeUs) { + + static timeUs_t adsbTtlLastCleanServiced = 0; + timeDelta_t adsbTtlSinceLastCleanServiced = cmpTimeUs(currentTimeUs, adsbTtlLastCleanServiced); + + + if (adsbTtlSinceLastCleanServiced > 1000000) // 1s + { + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesList[i].ttl > 0) { + adsbVehiclesList[i].ttl--; + } + } + + adsbTtlLastCleanServiced = currentTimeUs; + } +}; + +bool enviromentOkForCalculatingDistaceBearing(void){ + return (STATE(GPS_FIX) && gpsSol.numSat > 4); +} + +#endif + diff --git a/src/main/io/adsb.h b/src/main/io/adsb.h new file mode 100644 index 00000000000..86eefd8aac1 --- /dev/null +++ b/src/main/io/adsb.h @@ -0,0 +1,67 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include "common/time.h" +#include "fc/runtime_config.h" + +#define ADSB_CALL_SIGN_MAX_LENGTH 9 +#define ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST 10 + +typedef struct { + bool valid; + int32_t dir; // centidegrees direction to plane, pivot is inav FC + uint32_t dist; // CM distance to plane, pivot is inav FC + int32_t verticalDistance; // CM, vertical distance to plane, pivot is inav FC +} adsbVehicleCalculatedValues_t; + +typedef struct { + uint32_t icao; // ICAO address + int32_t lat; // Latitude, expressed as degrees * 1E7 + int32_t lon; // Longitude, expressed as degrees * 1E7 + int32_t alt; // Barometric/Geometric Altitude (ASL), in cm + uint16_t heading; // Course over ground in centidegrees + uint16_t flags; // Flags to indicate various statuses including valid data fields + uint8_t altitudeType; // Type from ADSB_ALTITUDE_TYPE enum + char callsign[ADSB_CALL_SIGN_MAX_LENGTH]; // The callsign, 8 chars + NULL + uint8_t emitterType; // Type from ADSB_EMITTER_TYPE enum + uint8_t tslc; // Time since last communication in seconds +} adsbVehicleValues_t; + +typedef struct { + adsbVehicleValues_t vehicleValues; + adsbVehicleCalculatedValues_t calculatedVehicleValues; + uint8_t ttl; +} adsbVehicle_t; + + + +typedef struct { + uint32_t vehiclesMessagesTotal; +} adsbVehicleStatus_t; + +void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal); +adsbVehicle_t * findVehicleClosest(void); +adsbVehicle_t * findVehicle(uint8_t index); +uint8_t getActiveVehiclesCount(void); +void adsbTtlClean(timeUs_t currentTimeUs); +adsbVehicleStatus_t* getAdsbStatus(void); +adsbVehicleValues_t* getVehicleForFill(void); +bool enviromentOkForCalculatingDistaceBearing(void); +void recalculateVehicle(adsbVehicle_t* vehicle); \ No newline at end of file diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 02d0e02a803..cc34a8399f0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -62,6 +62,7 @@ #include "drivers/time.h" #include "drivers/vtx_common.h" +#include "io/adsb.h" #include "io/flashfs.h" #include "io/gps.h" #include "io/osd.h" @@ -2067,7 +2068,73 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits, false); break; } +#ifdef USE_ADSB + case OSD_ADSB_WARNING: + { + static uint8_t adsblen = 1; + uint8_t arrowPositionX = 0; + + for (int i = 0; i <= adsblen; i++) { + buff[i] = SYM_BLANK; + } + + buff[adsblen]='\0'; + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); // clear any previous chars because variable element size + adsblen=1; + adsbVehicle_t *vehicle = findVehicleClosest(); + + if(vehicle != NULL){ + recalculateVehicle(vehicle); + } + + if ( + vehicle != NULL && + (vehicle->calculatedVehicleValues.dist > 0) && + vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning) && + (osdConfig()->adsb_ignore_plane_above_me_limit == 0 || METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit) > vehicle->calculatedVehicleValues.verticalDistance) + ){ + buff[0] = SYM_ADSB; + osdFormatDistanceStr(&buff[1], (int32_t)vehicle->calculatedVehicleValues.dist); + adsblen = strlen(buff); + + buff[adsblen-1] = SYM_BLANK; + + arrowPositionX = adsblen-1; + osdFormatDistanceStr(&buff[adsblen], vehicle->calculatedVehicleValues.verticalDistance); + adsblen = strlen(buff)-1; + + if (vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_alert)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + + buff[adsblen]='\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + if (arrowPositionX > 0){ + int16_t panHomeDirOffset = 0; + if (osdConfig()->pan_servo_pwm2centideg != 0){ + panHomeDirOffset = osdGetPanServoOffset(); + } + int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + arrowPositionX, elemPosY), CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir) - flightDirection + panHomeDirOffset); + } + + return true; + } + case OSD_ADSB_INFO: + { + buff[0] = SYM_ADSB; + if(getAdsbStatus()->vehiclesMessagesTotal > 0){ + tfp_sprintf(buff + 1, "%2d", getActiveVehiclesCount()); + }else{ + buff[1] = '-'; + } + + break; + } + +#endif case OSD_MAP_NORTH: { static uint16_t drawn = 0; @@ -3729,6 +3796,11 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT, .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT, #endif +#ifdef USE_ADSB + .adsb_distance_warning = SETTING_OSD_ADSB_DISTANCE_WARNING_DEFAULT, + .adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT, + .adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT, +#endif #ifdef USE_SERIALRX_CRSF .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, @@ -3968,6 +4040,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_ADSB_WARNING] = OSD_POS(2, 7); + osdLayoutsConfig->item_pos[0][OSD_ADSB_INFO] = OSD_POS(2, 8); #if defined(USE_ESC_SENSOR) osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2); osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 5bcf6d34740..8ce197f9bd4 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -280,6 +280,8 @@ typedef enum { OSD_MULTI_FUNCTION, OSD_ODOMETER, OSD_PILOT_LOGO, + OSD_ADSB_WARNING, + OSD_ADSB_INFO, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -455,6 +457,11 @@ typedef struct osdConfig_s { bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. uint16_t arm_screen_display_time; // Length of time the arm screen is displayed + #ifdef USE_ADSB + uint16_t adsb_distance_warning; // in metres + uint16_t adsb_distance_alert; // in metres + uint16_t adsb_ignore_plane_above_me_limit; // in metres +#endif } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index eba96690a27..5cd108e560d 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -99,3 +99,5 @@ #define MSP2_INAV_EZ_TUNE_SET 0x2071 #define MSP2_INAV_SELECT_MIXER_PROFILE 0x2080 + +#define MSP2_ADSB_VEHICLE_LIST 0x2090 diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 57707c725b1..0d91876cb72 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -70,6 +70,9 @@ typedef enum { #ifdef USE_BARO TASK_BARO, #endif +#ifdef USE_ADSB + TASK_ADSB, +#endif #ifdef USE_PITOT TASK_PITOT, #endif diff --git a/src/main/target/common.h b/src/main/target/common.h index d2d135cfa2e..d7ffd86e68b 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -182,6 +182,14 @@ #define USE_CMS_FONT_PREVIEW +//ADSB RECEIVER +#ifdef USE_GPS +#define USE_ADSB +#define MAX_ADSB_VEHICLES 5 +#define ADSB_LIMIT_CM 6400000 +#endif + + //Designed to free space of F722 and F411 MCUs #if (MCU_FLASH_SIZE > 512) #define USE_VTX_FFPV diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 187f959b178..b3c3509b216 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -58,6 +58,7 @@ #include "flight/pid.h" #include "flight/servos.h" +#include "io/adsb.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/serial.h" @@ -1054,6 +1055,50 @@ static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { return true; } +#ifdef USE_ADSB +static bool handleIncoming_ADSB_VEHICLE(void) { + mavlink_adsb_vehicle_t msg; + mavlink_msg_adsb_vehicle_decode(&mavRecvMsg, &msg); + + adsbVehicleValues_t* vehicle = getVehicleForFill(); + if(vehicle != NULL){ + vehicle->icao = msg.ICAO_address; + vehicle->lat = msg.lat; + vehicle->lon = msg.lon; + vehicle->alt = (int32_t)(msg.altitude / 10); + vehicle->heading = msg.heading; + vehicle->flags = msg.flags; + vehicle->altitudeType = msg.altitude_type; + memcpy(&(vehicle->callsign), msg.callsign, sizeof(vehicle->callsign)); + vehicle->emitterType = msg.emitter_type; + vehicle->tslc = msg.tslc; + + adsbNewVehicle(vehicle); + } + + //debug vehicle + /* if(vehicle != NULL){ + + char name[9] = "DUMMY "; + + vehicle->icao = 666; + vehicle->lat = 492383514; + vehicle->lon = 165148681; + vehicle->alt = 100000; + vehicle->heading = 180; + vehicle->flags = ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS; + vehicle->altitudeType = 0; + memcpy(&(vehicle->callsign), name, sizeof(vehicle->callsign)); + vehicle->emitterType = 6; + vehicle->tslc = 0; + + adsbNewVehicle(vehicle); + }*/ + + return true; +} +#endif + static bool processMAVLinkIncomingTelemetry(void) { while (serialRxBytesWaiting(mavlinkPort) > 0) { @@ -1076,6 +1121,10 @@ static bool processMAVLinkIncomingTelemetry(void) return handleIncoming_MISSION_REQUEST(); case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: return handleIncoming_RC_CHANNELS_OVERRIDE(); +#ifdef USE_ADSB + case MAVLINK_MSG_ID_ADSB_VEHICLE: + return handleIncoming_ADSB_VEHICLE(); +#endif default: return false; } From 69ba0da0eba3fefb8d0c0fc9a364ba67dcea4aac Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 18 Dec 2023 17:40:44 +0100 Subject: [PATCH 189/199] Update SPEEDYBEEF405V3.md Remove warning for DSHOT --- docs/boards/SPEEDYBEEF405V3.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/boards/SPEEDYBEEF405V3.md b/docs/boards/SPEEDYBEEF405V3.md index 7ec4b1ce944..fe51d3ce658 100644 --- a/docs/boards/SPEEDYBEEF405V3.md +++ b/docs/boards/SPEEDYBEEF405V3.md @@ -1,6 +1,6 @@ # SpeedyBee F405 V3 -> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead +> Notice: DSHOT on SpeedyBe F405 V3 requires INAV 7.0.0 or later. If you are using an older version, use MULTISHOT instead. > Notice: The analog OSD and the SD card (blackbox) share the same SPI bus, which can cause problems when trying to use analog OSD and blackbox at the same time. From 738ebb4fc9dea3728315cafde9721114003fa468 Mon Sep 17 00:00:00 2001 From: EMSR Date: Tue, 19 Dec 2023 12:57:10 +0800 Subject: [PATCH 190/199] make at32 adc more accure --- src/main/drivers/adc_at32f43x.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/adc_at32f43x.c b/src/main/drivers/adc_at32f43x.c index 1975a96e7b0..e2ed2f57b15 100644 --- a/src/main/drivers/adc_at32f43x.c +++ b/src/main/drivers/adc_at32f43x.c @@ -179,11 +179,11 @@ void adcHardwareInit(drv_adc_config_t *init) adcDevice_t * adc = &adcHardware[adcConfig[i].adcDevice]; // init adc gpio port IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_CH1 + (i - ADC_CHN_1), 0); - IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_OUTPUT_OPEN_DRAIN, GPIO_PULL_NONE)); + IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, 0, 0)); adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); adcConfig[i].dmaIndex = adc->usedChannelCount++; - adcConfig[i].sampleTime = ADC_SAMPLETIME_6_5; + adcConfig[i].sampleTime = ADC_SAMPLETIME_640_5; adcConfig[i].enabled = true; adc->enabled = true; From 03eabf630ac4fe1808da0d46ae898231443eb9c2 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Thu, 21 Dec 2023 10:41:35 +0000 Subject: [PATCH 191/199] update toolchain to gcc13.2 (#9579) add "-Wl,--no-warn-rwx-segments" for ATF4 --- cmake/arm-none-eabi-checks.cmake | 56 ++++++++++++++++++++++---------- cmake/at32.cmake | 9 ++--- 2 files changed, 43 insertions(+), 22 deletions(-) diff --git a/cmake/arm-none-eabi-checks.cmake b/cmake/arm-none-eabi-checks.cmake index 497828088a1..200065fdd41 100644 --- a/cmake/arm-none-eabi-checks.cmake +++ b/cmake/arm-none-eabi-checks.cmake @@ -2,21 +2,18 @@ include(gcc) set(arm_none_eabi_triplet "arm-none-eabi") # Keep version in sync with the distribution files below -set(arm_none_eabi_gcc_version "10.3.1") -set(arm_none_eabi_base_url "https://developer.arm.com/-/media/Files/downloads/gnu-rm/10.3-2021.10/gcc-arm-none-eabi-10.3-2021.10") +set(arm_none_eabi_gcc_version "13.2.1") +# This is the output directory "pretty" name and URI name prefix +set(base_dir_name "arm-gnu-toolchain-13.2.rel1") +# This is the name inside the archive, which is no longer evincible from URI, alas +set(archive_base_dir_name "arm-gnu-toolchain-13.2.Rel1") +set(arm_none_eabi_base_url "https://developer.arm.com/-/media/Files/downloads/gnu/13.2.rel1/binrel/${base_dir_name}") # suffix and checksum -set(arm_none_eabi_win32 "win32.zip" 2bc8f0c4c4659f8259c8176223eeafc1) -set(arm_none_eabi_linux_amd64 "x86_64-linux.tar.bz2" 2383e4eb4ea23f248d33adc70dc3227e) -set(arm_none_eabi_linux_aarch64 "aarch64-linux.tar.bz2" 3fe3d8bb693bd0a6e4615b6569443d0d) -set(arm_none_eabi_gcc_macos "mac.tar.bz2" 7f2a7b7b23797302a9d6182c6e482449) - -function(arm_none_eabi_gcc_distname var) - string(REPLACE "/" ";" url_parts ${arm_none_eabi_base_url}) - list(LENGTH url_parts n) - math(EXPR last "${n} - 1") - list(GET url_parts ${last} basename) - set(${var} ${basename} PARENT_SCOPE) -endfunction() +set(arm_none_eabi_win32 "mingw-w64-i686-arm-none-eabi.zip" 7fd677088038cdf82f33f149e2e943ee) +set(arm_none_eabi_linux_amd64 "x86_64-arm-none-eabi.tar.xz" 791754852f8c18ea04da7139f153a5b7) +set(arm_none_eabi_linux_aarch64 "aarch64-arm-none-eabi.tar.xz" 5a08122e6d4caf97c6ccd1d29e62599c) +set(arm_none_eabi_darwin_amd64 "darwin-x86_64-arm-none-eabi.tar.xz" 41d49840b0fc676d2ae35aab21a58693) +set(arm_none_eabi_darwin_aarch64 "darwin-arm64-arm-none-eabi.tar.xz" 2c43e9d72206c1f81227b0a685df5ea6) function(host_uname_machine var) # We need to call uname -m manually, since at the point @@ -47,7 +44,14 @@ function(arm_none_eabi_gcc_install) message("-- no precompiled ${arm_none_eabi_triplet} toolchain for machine ${machine}") endif() elseif(CMAKE_HOST_SYSTEM_NAME STREQUAL "Darwin") - set(dist ${arm_none_eabi_gcc_macos}) + host_uname_machine(machine) + if(machine STREQUAL "x86_64" OR machine STREQUAL "amd64") + set(dist ${arm_none_eabi_darwin_amd64}) + elseif(machine STREQUAL "aarch64") + set(dist ${arm_none_eabi_darwin_aarch64}) + else() + message("-- no precompiled ${arm_none_eabi_triplet} toolchain for machine ${machine}") + endif() endif() if(dist STREQUAL "") @@ -83,11 +87,27 @@ function(arm_none_eabi_gcc_install) if(NOT status EQUAL 0) message(FATAL_ERROR "error extracting ${basename}: ${status}") endif() + string(REPLACE "." ";" url_parts ${dist_suffix}) + list(GET url_parts 0 host_dir_name) + set(dir_name "${archive_base_dir_name}-${host_dir_name}") + file(REMOVE_RECURSE "${TOOLS_DIR}/${base_dir_name}") + file(RENAME "${TOOLS_DIR}/${dir_name}" "${TOOLS_DIR}/${base_dir_name}") + # This is **somewhat ugly** + # the newlib distributed by ARM generates suprious warnings from re-entrant POSIX functions + # that INAV doesn't use. These "harmless" warnings can be surpressed by removing the + # errant section from the only libnosys used by INAV ... + # So look the other way ... while this is "fixed" + execute_process(COMMAND arm-none-eabi-objcopy -w -R .gnu.warning.* "${TOOLS_DIR}/${base_dir_name}/arm-none-eabi/lib/thumb/v7e-m+fp/hard/libnosys.a" + RESULT_VARIABLE status + WORKING_DIRECTORY ${TOOLS_DIR} + ) + if(NOT status EQUAL 0) + message(FATAL_ERROR "error fixing libnosys.a: ${status}") + endif() endfunction() function(arm_none_eabi_gcc_add_path) - arm_none_eabi_gcc_distname(dist_name) - set(gcc_path "${TOOLS_DIR}/${dist_name}/bin") + set(gcc_path "${TOOLS_DIR}/${base_dir_name}/bin") if(CMAKE_HOST_SYSTEM MATCHES ".*Windows.*") set(sep "\\;") else() @@ -110,7 +130,7 @@ function(arm_none_eabi_gcc_check) message("-- found ${prog} ${version} at ${prog_path}") if(COMPILER_VERSION_CHECK AND NOT arm_none_eabi_gcc_version STREQUAL version) message("-- expecting ${prog} version ${arm_none_eabi_gcc_version}, but got version ${version} instead") - arm_none_eabi_gcc_install() + arm_none_eabi_gcc_install() return() endif() endfunction() diff --git a/cmake/at32.cmake b/cmake/at32.cmake index aa902593e94..54e178deb7b 100644 --- a/cmake/at32.cmake +++ b/cmake/at32.cmake @@ -9,7 +9,7 @@ option(SEMIHOSTING "Enable semihosting") message("-- DEBUG_HARDFAULTS: ${DEBUG_HARDFAULTS}, SEMIHOSTING: ${SEMIHOSTING}") set(CMSIS_DIR "${MAIN_LIB_DIR}/lib/main/AT32F43x/Drivers/CMSIS") -set(CMSIS_INCLUDE_DIR "${CMSIS_DIR}/cm4/core_support") +set(CMSIS_INCLUDE_DIR "${CMSIS_DIR}/cm4/core_support") # DSP use common set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP") set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include") @@ -50,8 +50,8 @@ main_sources(AT32_ASYNCFATFS_SRC ) main_sources(AT32_MSC_SRC - msc/at32_msc_diskio.c - msc/emfat.c + msc/at32_msc_diskio.c + msc/emfat.c msc/emfat_file.c ) @@ -92,6 +92,7 @@ set(AT32_LINK_OPTIONS -Wl,--cref -Wl,--no-wchar-size-warning -Wl,--print-memory-usage + -Wl,--no-warn-rwx-segments ) # Get target features macro(get_at32_target_features output_var dir target_name) @@ -264,7 +265,7 @@ function(add_at32_executable) endif() endfunction() -# Main function of AT32 +# Main function of AT32 function(target_at32) if(NOT arm-none-eabi STREQUAL TOOLCHAIN) return() From a54da6a370e02b62c6e67a073c563c61e14da2e6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 26 Dec 2023 11:52:18 +0100 Subject: [PATCH 192/199] increase default nav_cruise_yaw_rate to 60 deg/s --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7483f145acb..f0c57f4273b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2695,7 +2695,7 @@ groups: type: bool - name: nav_cruise_yaw_rate description: "Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]" - default_value: 20 + default_value: 60 field: general.cruise_yaw_rate min: 0 max: 120 From f6acb7a7a2342615060f1b65ae4006e5c9f177d6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 26 Dec 2023 12:02:24 +0100 Subject: [PATCH 193/199] Docs update --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index bc6dfef4053..ab76a937158 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2818,7 +2818,7 @@ Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on | Default | Min | Max | | --- | --- | --- | -| 20 | 0 | 120 | +| 60 | 0 | 120 | --- From 740c7e06c145236eedc56efbf771cac302be3706 Mon Sep 17 00:00:00 2001 From: Stefano Della Valle Date: Sat, 30 Dec 2023 17:34:47 +0100 Subject: [PATCH 194/199] Update logic_condition.h --- src/main/programming/logic_condition.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 8869694253a..29cf8353af2 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -82,8 +82,8 @@ typedef enum { LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_APPROX_EQUAL = 51, LOGIC_CONDITION_LED_PIN_PWM = 52, - LOGIC_CONDITION_RESET_MAG_CALIBRATION = 53, - LOGIC_CONDITION_DISABLE_GPS_FIX = 54, + LOGIC_CONDITION_DISABLE_GPS_FIX = 53, + LOGIC_CONDITION_RESET_MAG_CALIBRATION = 54, LOGIC_CONDITION_LAST = 55, } logicOperation_e; From 09f6a0c697420cb35ea045da028b4de121555ba7 Mon Sep 17 00:00:00 2001 From: sys512 <145368759+sys512@users.noreply.github.com> Date: Tue, 9 Jan 2024 06:59:17 +0200 Subject: [PATCH 195/199] Update GPS_fix_estimation.md Fix spelling mistakes --- docs/GPS_fix_estimation.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index d8175549d35..5d11e626097 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -35,11 +35,11 @@ To navigate without GPS fix, we make the following assumptions: It is possible to roughly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). -From estimated heading direction and speed, plane is able to **roughty** estimate it's position. +From estimated heading direction and speed, plane is able to **roughly** estimate it's position. It is assumed, that plane will fly in roughly estimated direction to home position untill either GPS fix or RC signal is recovered. -*Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. +*Plane has to acquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. # Estimation without magnethometer From 43c17060bd00e59c0e6caac2308d43a340c8bfb2 Mon Sep 17 00:00:00 2001 From: Hugo Chiang <31283897+DusKing1@users.noreply.github.com> Date: Fri, 12 Jan 2024 09:44:38 +0800 Subject: [PATCH 196/199] add support for skystarsf722hdpro --- src/main/target/SKYSTARSF722HD/CMakeLists.txt | 1 + src/main/target/SKYSTARSF722HD/target.h | 24 +++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/src/main/target/SKYSTARSF722HD/CMakeLists.txt b/src/main/target/SKYSTARSF722HD/CMakeLists.txt index 9aadb3609b3..a13320d26ac 100644 --- a/src/main/target/SKYSTARSF722HD/CMakeLists.txt +++ b/src/main/target/SKYSTARSF722HD/CMakeLists.txt @@ -1,2 +1,3 @@ target_stm32f722xe(SKYSTARSF722HD) +target_stm32f722xe(SKYSTARSF722HDPRO) target_stm32f722xe(SKYSTARSF722MINIHD) diff --git a/src/main/target/SKYSTARSF722HD/target.h b/src/main/target/SKYSTARSF722HD/target.h index c24b849182d..d46e2cf162f 100644 --- a/src/main/target/SKYSTARSF722HD/target.h +++ b/src/main/target/SKYSTARSF722HD/target.h @@ -20,6 +20,9 @@ #ifdef SKYSTARSF7MINIHD #define TARGET_BOARD_IDENTIFIER "SS7M" #define USBD_PRODUCT_STRING "SkystarsF722MiniHD" +#elif defined(SKYSTARSF722HDPRO) +#define TARGET_BOARD_IDENTIFIER "SS7P" +#define USBD_PRODUCT_STRING "SkystarsF722HDPRO" #else #define TARGET_BOARD_IDENTIFIER "SS7D" #define USBD_PRODUCT_STRING "SkystarsF722HD" @@ -45,6 +48,22 @@ #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW270_DEG +#elif defined(SKYSTARSF722HDPRO) +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG_FLIP + +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG_FLIP + +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_BUS BUS_SPI1 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW90_DEG_FLIP + #else #define BMI270_CS_PIN PA4 #define BMI270_SPI_BUS BUS_SPI1 @@ -81,6 +100,11 @@ #define USE_BARO_BMP280 #define BMP280_SPI_BUS BUS_SPI2 #define BMP280_CS_PIN PB1 +#ifdef SKYSTARSF722HDPRO +#define USE_BARO_SPL06 +#define SPL06_SPI_BUS BUS_SPI2 +#define SPL06_CS_PIN PB1 +#endif // *************** UART ***************************** #define USE_VCP From 61d8b2fff4493b96dd88f3b36ab8ae3b48ce80f5 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sat, 13 Jan 2024 21:19:12 +0000 Subject: [PATCH 197/199] [docs] update dbg-tool link in serial_printf_debugging.md (#9639) --- docs/development/serial_printf_debugging.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/serial_printf_debugging.md b/docs/development/serial_printf_debugging.md index 6cbeec6cfdd..3804c948e9f 100644 --- a/docs/development/serial_printf_debugging.md +++ b/docs/development/serial_printf_debugging.md @@ -100,7 +100,7 @@ Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messa * [msp-tool](https://github.com/fiam/msp-tool) * [mwp](https://github.com/stronnag/mwptools) -* [dbg-tool](https://github.com/stronnag/mwptools) +* [dbg-tool](https://codeberg.org/stronnag/dbg-tool) * [INAV Configurator](https://github.com/iNavFlight/inav-configurator) For example, with the final lines of `src/main/fc/fc_init.c` set to: From 336b31d0ad5ce43ce3a89d0dc86bc8009bdfab7d Mon Sep 17 00:00:00 2001 From: Paul Fornage <36117326+paulwrath1223@users.noreply.github.com> Date: Sun, 14 Jan 2024 00:48:45 -0800 Subject: [PATCH 198/199] aligned KAKUTEH7WING target with manufacturer docs --- src/main/target/KAKUTEH7WING/target.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h index 79068b8cd84..b4ea4a03bd5 100644 --- a/src/main/target/KAKUTEH7WING/target.h +++ b/src/main/target/KAKUTEH7WING/target.h @@ -109,11 +109,11 @@ #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_ALL -#define TEMPERATURE_I2C_BUS BUS_I2C1 -#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER -#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define RANGEFINDER_I2C_BUS BUS_I2C2 // *************** UART ***************************** #define USE_VCP From 50b83cb3ee4d9999a9bd57c292af4a03999b1164 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Tue, 16 Jan 2024 12:02:48 -0600 Subject: [PATCH 199/199] Documents i2c usage on KAKUTEH7WING --- .../images/KAKUTEH7WING-wiring-diagram.webp | Bin 0 -> 292996 bytes docs/boards/KAKUTEH7WING.md | 10 ++++++++++ 2 files changed, 10 insertions(+) create mode 100644 docs/assets/images/KAKUTEH7WING-wiring-diagram.webp create mode 100644 docs/boards/KAKUTEH7WING.md diff --git a/docs/assets/images/KAKUTEH7WING-wiring-diagram.webp b/docs/assets/images/KAKUTEH7WING-wiring-diagram.webp new file mode 100644 index 0000000000000000000000000000000000000000..35dbca48ce63fe7dc5215078891d0be79a631c5b GIT binary patch literal 292996 zcmb@tb9iP;(=Qy`wllHqOfs>JiEZ1)L=)S#lZidCZQFM4li7Pe-}AiZya#`L>$=u; zSJz!@b*<{|>gw)aDN2ZndbxoCsfh^5tIBh#!vFyRQGVXIkU#)PAPHe%`4ZsIA`ozi zm7%pG2oeyGwT+X5qL>hox`rkZhz~BI9DgTKeYNMjpp#F)F*BAhc^A2_WMH{ z{7onPC8Mk;{3!$XNei0(2W{{l^gnfdiU5HdG8>xzA^vq4Iy<=hL*sn@jsg#Z8iL0` zS;9Ub37{+BvJziY{ba`Ci4z4^aMeDywDSNDuTAr+Fz@o7?K@@!0Gy27KJ^6x5*B6z z`eVaCG!W3gZS!xQZ5|L1=RFV*=+A%i3{ruBnuCFWuvY%fBg+8-LJR@|YMT6PL83Ef zFrX`G#7tmnJV+oYeuXbFBKdjb6l6{>ep@im#x{?G1P8skt?hsn_oBDMBWlWy*^g!j z1{Os26S%4B1HuPBabI!(&&eo&|D**jGxVKMlU+-`Ol9#2x`DR$HDuJc5jQqF|90a9 z`y9^i1M<$d4IuKGz0Jb>R{lu14{+-0`8fAFzqt$}czI)ZL44KS?Q-zB5a;34ng62BLMb; z=FRju+dZ53gYYfp$@Z4;sH+{Yb-#GsRp<-PA5S{?e0Fm$ej~sh3PlS5{lb$9Pfpy=ZtEDyHb-)@x;iGMhbZN58w;lle;rMa+ zth)h7XUGHOetdtA0U&My5&)_IXaMG$?n})3XA=PclkdPE+V5iTu8)_uo6o+3fDgd@ zWAp3fhwiz6kGEjwJOB}Z`Jy{EStD>Duqtru8w=2Wwg0pQV3*zZ(UsG=Qe`j<5GLx&03jU5?R>h8P@JVWo!ajXu~8g8cBEeEO`?k6%MRlS-+@|CDgCKFC_K^(6M4uS z|HDcwlMankD0h#4nPVeg+(6O9DJ|=#QjcuD4dWGwIf5W6$Z!paobza0xPvc6sczh@ z>PZrg{_~u@(r!TRqY>BIS{ec;Fh&aKY_~@erEhq7P{`((YTa$abx?KpkQBCFSYc5l z{5bY7x2jO#;%>70E%n+oHGG5h;OgUZ$|QLg9Rj*@2Faee2|SI1SNB08-OY~K0;YFK z^S)-eb&Jsz*evq;nluy(UzyO*rXeo|?`*7XZmy}5 zb|Mv3Hq69GlmkVN=AXE3V#1610^%QuX#B?6rtDiY@g+8cA~%NJe(2BCdQ4fEZ_KU? z<^0Xs8_o$&BAE1z`L%+WGb!VuzTy#u^dEclG_r?GXo1ou0i~wPqpLkq?9uKu=nk=U z&3tO23_LQUws_kUg2^%ZdMnkS7cZ5Svr}wF>?is@EtImT#(?oaMWW+$GhX(N!grKi zSQX?E*ho5M(N|*$Jv`#1N^ zIBl$i4^Hx_1qJ`PP{m!L0r)a8+d%smBLv6kk^jmY%atr8*Al47o!FYTL9CP1=a-97 z(la(H;3aMT=O}eTh#_g-e1_F*!wLAbk^Vlx>Ty}bN#HEVyhzkn`*4kGdO767zn{$c z)jKaZ-;WPZ9e;7@>e;rwaq;Nd*0KJ-Db!fRDaXahSx+SsI~r|&M^}_MZ$;V5BD=

S_Ku3VHRod!JEI{cOl5S5Qh$mbIEoFR>$s^7oSz;k^HS;c5fIb)x~fh6{w% z+_3K_sb~`K|8FF*jWVAi`SE39sVJg=h1Wj41^t4RCjxdmsn9468MKzZMIP5z`{1&$ zeemBHWuyt1Sehl#^8Xih_J|kl(xlj<;|}j$e!|j~#eh`BaL=|&&YejWKV`xeq%mi= zlR;Wpr)uGUbnCy>5Kk`tKeN?PkYhgDnY>PfW=sLYi(={aFE7hHyhhhJVCcBho-ElC zb}SX@r}gm{Kl2VPNw>67ZcdR&|8#?I4u5*f-y`aue)WH5Z>``!@-?Q|>gNNO&qFGT zj*8{&jaM4^1z{UnMF)T1q;EN1cSb?T&{9u#y8xY~H^){%Y9icNjZ^>f<^RP_|HZ1o zN00?g@%>d9prA!LBpm3$DU@A+#pg$8pr_H}J3?y8ap1>|ct+X@Qh(bz%|) zMyU5Qt!8jBd>wZ{Hc3;oZI4|vAwJ+;%6an?_em*vP+unLnmgzpVU3!e=v~t1pyDz> zZTszHqUu4vn1vBTf=;Hcov01RF@IZkDh!L2c8q~(tR@WqqCRd$F7*H25Ezw%`&FVq zp5hQ9!+i|#*WJhI<8TfNAQ6N&UElMSLj0Hv3U=8Ns^quQ!OfW{B`f{y4Y;P;r5dM< zdM1{_O9&Ca?twu0!Ou7l_vjg5djp0S!kjfvt`cCyN#oN)cD8^wjb@+#8fA4%5;;c?sr4C+laC%JiAxba&D-4jc{T4-KBWToH1G^=<2#6xov zDuLgqdo_N!70pdwDz4x%o4eL0{D&j{954p9*s>CIRUGO0DB#5sWY|6!I)7aB=Rdjv z*qq%S9n0@3Mb;o}2%v>0H5-2a3bGV2*}?W9Wv)$^)b3=S`x#Odq9{g8zIoqrh#prE zN~4NaW*Kz1{2Bb%W|d8)$goK>qEp}$y9V-7)z`=WHE1hZb=D4Y`S{I-OjC5)#5UH} zi3(-={K)%=SuoOQ40|4xHF=y$`*&!V=4AcU(SppyUP+|y?O^VXq{}^sDH-KY}rS+XWU_db26u#nWj+?GkWd z=?b>#Gxs@mHs8o=QU6_&_Ptu2%o&oI$ot4*Jo|sv-iVg@3U(1mrJs6Ap6%wi;&4DY zS|dPV35Hr!3he3C^1W0%D}qcN&u^(YmOOxtkm?j39?oTG$_G7x{KMf?x7i|(kLd5n z7;XOX;>I;Nej~3_cGhp{{Thh@BL3I>SKH(X4F0$*tA>6=wP6xN{n3P%b-H;V z!iPVMw!n2KH93B8&ShN_3qiixH?z#do1qM5}FM| z8Opvz|1y!2s83CFuR4d+Y-gkTp- zWUDq(et6P#9KGf_Brdy!znx3jva(BADP~cg?QyJJdNMtSec(pLEaY$p$lVHmBgA@D zENDwo9ZVAb?bKz%XR8=S^Tnv8EyHFDW5+jLY@FtE*w(-)TxWel%~n#6JC z($P8GoEHW5<>#~!zi*#S3uZz|j|8<9v#kNuWo3W|sS`{Y^8vH@BdV)eEJAXBW)1tc zq~j-WoH}^qxz^2IHu%kn79W@2OiC6YRo7eMWivWYG~sd9WX<3?gCyj+sK(t*F_C!^1q7uBbu-gF@Ydxr9p zQuf)5CTL{!9RBOW*tW6JHPusv{^6W-G=`7>a|VA6Z-15mEa9os6lA6!tA zb%x6goXn#t%jB)MLXQ#BA3guqoHcj-tfV_w*7}XwI=<$d zk$oIQf?+81h~&uEy@Qs znVvBXzn|^Adqf}|SoEXwtx{iVn;3UD7 zn)9Bu=h`we0_B`(yo~8A17F@Ih^t4+w#Gwv)DWx1rzZ3VDx>p6Iu`(rSwd4w#2;-t zjJyQD(xrjeWlO4>4Wj@YPpkTNNtk1Z{oS^ps%vZ{zyva}OMva?>0Xln<~1b>?D1cr z9;%;OmPeq{$=9hi-%|`et@|(i2D}WlBuBz|^ShW|(1TEN-R-sXhy)9|6(mp4q7bCC zoMZ$?JYmOTaj(ycRo|nZR0U??2e=jcv&2g2n2%al0^!OFY!gfWOZaX^;)sGH6=BjB zr@xI~Lem6dN$~S!`);BC2pIW@0`72OYFq>b*b)~9tEl$=>!g*}1yb)X#Vvot2dM+D zxc4h%(vr^e5-SSu+t6aQBAU$Saxf5eC&57}lao52?1&)=@VO`|~4PixFLOzC3dl;uNj6W={MW zkXkCjA@)*u-R*(-XlQZ=ZY9OZZcdy7%Pla;H=n>DdvNu1jUZC};JCL2dVzTD)}uYS z@9~gBI9Gpr16|8!eV|HV+~^p+9jm%bRR`X!Y2l*%jcB+DQQ1C3qv*g~la3{Pb zSX2BB*v!ySffi*^P%~MAJ9ZN4dge@g%5RS$XHsgl5ef`^AX>5}{uHNJ0jUh)VT zz$f6Py7a#1q&PyHI%aP+8pfrLM-W%EuLOJu{cK5ln1YoIEQj<-OLlB5BPYFNA9M#$ z{VzCk_q?38Vf53|AOf^_bZ|AHUUX?pbc}LA*58b-7QJ z<-LXmEKk`Zn3rVu$ENnRX19-mQ2qB##B@vM6C_F3e+Ebf_JZiBG05esv5RxR;8pXi z?ZYPcKd;-|+Yq2Le-yH|pXI>8`{m!sre`(~fpQG|HLfX!YR?#slR=%BZ_W z961n*nEY_mrpxh?Gnp8)$`@$vv)u>z4cz-?Z(@RZ(Zf29J)pcVqz_W z2Hnqp+}rk3`;N^u`gOi8ygrgVrUY4#aFqr0g% zQaBB^)$Y3Ogxf?<^=ZHjn*-24tI?Al<7GRa*V-t*cW?@z(o!VR}c ze%^CRiXr^I+%#+H+S}6_a4E_Yl0YRS!a@$Z&Dc8s&Bnd(o+rC!43#0u(67zYW;Yr% zAiR)Y^H)h^;cwuevoE5Z-VBdU_$d9Y7v!5i>hD3{faEKz8=XcVio9f{xAje}7{fLg zI*RzQ!e~{VXpXd8dMIYiYlL}n2NxoSsCWz8^K+72^5U-f8yfjKxxRT&{Z2O|Ip4T6 zSda=)@uL70h2G;yfrAu3I(ws`Bz`wG^}RqK8eH__`ev+He#;AHCX*RV(%2@BtGw`G z!quSrM&8->7CMjKko-D2eVe7^#|Pr;m`}(JZ1kd6p593*@u5VJsI1T%BD}l7^@_}0 zad2)21p3`U3Z-3yW@)B_;_*(uXfvqX`cE7qh?8rl6znj8LnELXoWW*$nxttUHa#n! zlq3X1xLbtJ2CAc_%41Lh)Md9HNHj% zyC;}Ao`Ewj-(ROXW&4d*&lp9N_^{o$b8;yvoZz;bGIm=eufy>NR1|LW;oEq|>4NW| zd^c6AblZGo;5)1MJHC9}ZUir(jo?$iMe&8U$V6M6Lo@D}GisT2Zl*4rm_c5y`Y+N= ze?}$TEOYPA;GIK2@qr{n|N0g&RbY%LmVNB#aPQtZdf4nNV(CXGiTx(Ok760ehvJ&( zflCDv42=ttO1LP^)A@N_$?A=Tf?` zZ6~Kyh&C)stL3o647c-Z<+>|x%=z?WdFH@tNKQ34XBT`40`9Jx09^zNwA#dHGm@*) zyytB(Fz$xUw!oFCmzSTbCzm;W0mJVFju-i2j4n`*wl5Fgk)v6mx)kg)kYk~_?@zu3 z$olJ3xIxcP9D4Sk+Qu2&e?T1*O9WG{5Xu#OhZgC=u@)iHHXm|GE%`1|p}sFwSuOeO zq17y1Q0gdI;Sp5$ zlM28@RhBR_j(f#(n_Ja06?@&cm1Y*n3ht?xVSN7qNe3kqR7iQClY`8c8F*?H@P4z( zm&2A3_>G;1iSi=x>M`gSxnV>3Jn49my24_&u>yv97R&ywat4sd?T1q#6o-m@^Zg`E z7Bzuo5!RVrPVNUFyzPv2z>6Ea;~T2r;1UU$?_5h zt^zewTe1@|l`>fKk@-k#ypWc+7X_~L2YGWx(rUfUVn|4!B%_|v#P&9O{-;g%m~?lO zcJH+Mz1GVFZRkO$IRPTvr5EA6_q?gTng>r#$`v`LCvZmJE^U^(TQ!V$XKH8K>oXR@ z5VB>Mo|2PdDP^^>(0OQ7MT%1=yceWV`Kz-g{fmU|lOlRmIl`=6e%xENXt^EBpirM< zs#-maNQg{Qa+45#>RVB~NE&oxjB2R*@4i6kB!i#>oj*JFYp;nX6BPR;n=}03H<7y? zYdc?82-5DVDLI6Zd{O+o`K|S3BjMEkfo$iS|v~d-ME%%)ILFho*qQxyN%rO=SrszEKoa?c1u?+rhIUI_vHHO zq|F9fwz3(5w%3T@%SVgmQfE>#I&pk2ITy>5$lNb33~&$TYfO-4~g>{ z>!wORL;E)wC+hmC=Xy~)a)YO85n`%{gefNXq9X-o74PRxiKn>8hk!bS79*IjpJKQ! zFq^CuX@XhU^x)1v8Y+jp^4dUg%%W2+L{fSDGI^SF-}n-GR@8Z7ERUo2Y~!XQ(sebl z-#DF}=lPKltgY>`GR@VN-5OoNb4Dl4!uIuQ-xg2RhBjc>67X=ynXlKv)`57jEtdHU zxstrXGU5f&Bp&M|-2AT)2nJ9;VnO!6NfWgYu3LHRy|LQo4VuTL0ji+Uz_|#`xb7B$^aq43^Eyzd?QsUho$SMh;UAAbk9{~I8>T_P z&48s9X;x&qjU86A->O_G^l~qtM6WGWige6Dsv0hV{bpewvj?)Y+x-2gui3^zQDi4o zS92|#y3e*B8v*EB%N|}O&^=J&c^p{Pu7<=Qn>Ivqc+B?6J!Ww2nMOBCPx3RLO=(sq zSC6MZ8b$-$Qz`)}7X0mX63lJZ4?meNa`urm!g+c%3fsfefAOOoMCbfGU3>+?>h`qd z)#&1V02}?a>!G>nPKQ_oDy-@E(~1T0NjEka>wmIYRRhAa{61H7H8#)Au78l$ME!O=H z0zYaZioL|w*f@^NjXnf{!o#ieSO!{u>xq-({6ZU^I`T;J=&C&vi0&Sd zZ5qpBNfx{g>&>LLe+j~Ih7C9317{7;4eu9uhO{dKg+Gcvyf{5_YOmMVC{D+Mu#zl^ z_YjhgWMpHR23JiS(l3@# zGJNxZM3orIev~S$FzN(Nq}wX+^G2>#V7%bz%7@$DKcV+zq zqRdM=SANmqxJ$>fmmmr(br9Buu--epSZ89e&Et_K$U7|dg%=$c+F#B_y5Xn! zbYUs~EJ$)KAx}7P(&4N8jIM*7zIhtkwQt#+<}%_kzzGAMq<7mE=9Q=Nzn(`6P;{YA zW5hgoocxI5!8Gh!&i;_X+`Izsy7y}BxFbuAW__9RR8WUuND!*;gDqRd$HGI6z?nEP zO+xN2787x5-=7gPJh2$b3JyP@ZxI(fzt{y1JjvRzLOvF4MQ@1Dq*7?<2>YT(c_7sl z&FsP!unlY<0Rb|D(wyG_62D6DD#?dzF6hZNn=0qRmy%25JsE6toi&>w8$l%BPFX7Q zD(;kI;_^@(6qC|I7gE4IiwDuKMFT~!MqnA&I-hKZpCfDl;)m6jy=rDaL=&FiXtG$7 zlF%13%g`uQh#0ud zp5_;UHyU@ixy*kMNQT~SK9AP@MDv=`AdzzkS;j~Ej&Q60XvIl9`DV)#NR|s zz_5N_=XI+!9RHGnh=skW6u5$@j=3IqskgxE|kaA*z`%FdPaAMrBY4fdp?f zR(mbj^bGrnM9vHi4OlLtLu~Y2QzjXDkt1zTkoyn)p)9?8w{89MNF?A9iH=&hKm-Ko^5>e%RlV*h0)=`m>aHuyBXRPrkpWFBNU)RYX(RcnWG{4 zkjZV<@StuH2;bFwM<}toZRJ#AqGeqG3wh8eejIjbJZlMhm$Ahe)$kwzHL9fSoGkv` zoTqD>M*1M*;)QVtI?Rh{S@aLk%iHu)zaYqh@gazbjua?#W@m5u{iw;zH9zF)YrF-x zO1eXkuQK{we|l9J$#cC|g!x`wsBf#Mi3^yj_ruoS_$vJjy{46(T1Dc1CUSaT`}Rqg z7tIlPHqNnx?>lE4`=I(kjSsdSYeu9|T09jrFeT-o=PnB~$Sj5^Nlpa-T@(FHyHNNx zT!X{9s-m;2BUF_ zQp!$M$za}2iO=p;C2d+3@p%#QW1+N{jdbG0uoS|EZ~kR$7IY>ZcRFi?!PvOtum+>W z`&eqmIpuUZHW_@3NTm*gz;BjwTMa{No1rFX^|^~0h|DQY8fD5ay~Q?;W@Ax_A;7Ub zk@V6BcA^VQ-bBr5Si(O+qFDMU2hcoLGUwauXQSAFG}C*>RdIEc+ThQlx)WwSGjjHi4rs+5jz%(a@MA7;ZSCKP z=7Ek%a$Vt_!Bd6MRu%RwjpjoY$c_s<1F%}+I;r$B1vxxnJAJ^?&0iJ=nriul3dX2Q zl6_NHNp5>BvISF()CJRe(;(v(J07vZ^O;AyQT@NA*P=>UsFI;@>3B&Z$d>YK8D-W*Xk0WJn9)BRk zx?6iRG$H=_c;&whn$CX~#9IUd6dh^j#g#nid4!>p953-&>fLJ8Sxdpzhk8}5=$Koa zrW}~#R}0jo@DbY9@T6Ig0w%sq#dD@?F-T8fPV@y`lP~cffTOiQJRgJ~RvE@ZCK|4g zP^~w9!T_CoUR|UZI2GMC&z_daLR7e%!g@W^cjvJqh=fcs1oUnA7`lP^84m!gf&A@Y zcTGcP!KBA!9WhJuNu7+c znsku4d>fPS!{qAytu@KQ6a3O>?%Zn~3b%bkun1xS&x=vv2a#LN{ z6DX5Y72gWQ$xrd@Fp!@&V`7L1Diz}O2kt)nT5gBf2#KpdM*jvDDZPJ%vwn+AjRyZJ z^eE;3qQ@TI1V#zZA=;w> z*(j|@A#}^TET!kw1WSJ#(Y2f{b@@7pvr(HbU?5(_BoXJTD_%r|OH}y@T8|}QF`}Be zmvQdAkM_YN;;iQcaG?xs8Kd{a+-AcF--zPwZRMn#-n-~vk`#X&2+MDal8aWT&q?G^ zjsves$rAD z#?CHiCie&8(q3{g-wfCT&zad!JkCD35=j5?L>$+>3lU7yAaRoiZl**Jb6tNhi_NRl zJ;(vfzXx8sppjuMtYO1d3vbfiN}@X88M;q)9Y-dx3Dze?$^^aG7~%lopjnSR%ao5^iB zWEW8F?UiE?uO;@qM?mF0Rub5N5VT7AnxQwNsOq4kfkNiIx2!#ITAB`l)r`bwW><53 z{kIZ~ZdgdXbsA~?5C)gsD%NS02A5y|we^DLm<13%H%UX5$SV1*enX++eB|^n z$0%hEu#K0xWtqY|P^|NSiF-dTD{IP`sgi`ym)DWCdst<;*cou17^VSAFo#=oNL*G| zBsQ+QnDSMmy+jg(}oQfWyB)9VEyZ}u_A`NT*-Ud-)T(^kbc?PtFW4Tn2L-Z zO@tcgzI&gdgbcj;ocuwSe}_*=MrZaf-*6xlXQkBaaH~;kqcY>ozsgv$6RPm$PV{Ij z(D(&s0|uFw{o_>~=4v-LtK450RQ2VVY7-m@Pl)P<-JzIaWz%j`pD``0o&E>rxvj0w z@2BpPx4>cUn|1;`VR74qq?}|@<0#Mh|ZyYP3A_l_yA7^4dj_ZPv%)i%io^ zQX@L(f;CA$H)`Tn;cT1dY`eCXOBQ-fjWUrOC_TrxBB4K~VZP0Bfr~3cf>GHS;Pt4# z)&jF6ts3h09mhM)+Aki1;i|KZy{ilojav91dT|1%=zkhn-%>*M&KjIYyq%*{`{3br z{myf&@Zaj^z;bL|D}X66YE*d@K)5-$??6}q=J8T_@kWVL9&2XwI#=OJg3J7wykJTZ zaZJ9l9kx}m6H^c)<7ZrOavgg1E%HH-#;z7be4|QTJi0{qh2NP3gg^yQ*4u?g9Kczb z#?mT_+eX#5-HOuOQdT9FSr8X`FAlPKlOnRwxvoCxCWUKc?rVeJ%-bZXwgqb~P`?_Y zi)$s*mfVIZOT_ZU(lC?E>~ii@464A3Xl;YhJ$T4LYXDSnuqA;Yroa?Mf7%Q|GqP>R z-q2{@ltg> zEp;YRMuqjU(O>Qvht~}kWh=?CyCCO>u7|zQ_tixPd)hXd;iV;0ktN_*{q&&;g9P~> z?z&O+QxDmvLayX$xO!v5cSH(z_!G)Tk|r72yfO85RZN`2{5T|?e4|BfUw~=Pz`3r3 zVk$^;^3Ai>`{uL4#Q0N2M2D0l{Tbj z201cA3~U^FDJt$UUw*a%=c!RE{-&`FPLNxNH^?xf!Ma7kzh^3pR})pzk!FxqLX{4u zG;_Aq>g(K< zKsLH$y1|kgR1^yH=`?Ouo%T2j*Rg1m~K2D5c)5Jb>$-H6A* znuMa5=BK;z`RCW)ArfVjSmDp5sW)F!dQD!)$6ay(DmICz>tD%5sC%zQe&elub>#SQ z#(9img`fdp^0R83;x_*RnXlB)KaGQ!Tw71k0XZsO>EpK1{M48${v!+Ai^R;_jmk%( zj?$B<`@IOvsaKeTfOaj}&M85^ru1LvBjS#?Je0)6|H|$hJH8~sI`am!c+^N@7 zTgg^B@96ErP$(8JY{kDYi+xuJJOV|2lhu->H@JAC_ zv2~>QX4oK;H+viS;c3wpZgFst6d~j zAiSyHYlTNBib>$}hvz|u&Rxo(9P&Lh#X1?}hnEMF`%g_Md6UD&A3VJNM*UhhZzFs| z-aGgUIVA4xg#4G|fH0&D<6N~%j;4!8AygfEtA|OyxQ*OjhHGiKA2?VW3MihU0)Vim zGYnYre#I3T&QMfeHK*~xK`*6!Cm77iwl7vtw>o?HwO;m);*7faT9r+apj34lI^EPY zYtnP6lw^&&P9Dlv0xWMOIHliIEDolR5x^=7OD{QU#g2dwB6Tywa*yJkgBq=Z-aqIv zr?~W*(G4-l5s6%Igj7No2lp9FV;*sf>wBon6;rBdM0cv-@I~=qav38m+JOyg!)^xs;qc5;qfi^rWrPqOW3-h$~Fz*mXz5axRl*syCk=?nfXQ!;&YK(@i zl!yosEk;5(DOkWmU;|RWklKaThE16YV4>Q;;AUM?6=YHaVelkjnL8xeWg{Titj1*P zu(_FFnsD>e0VjKNPlrddUW}stvV_zKz<7J5Jz^ct+{44JZfbdJ#}mO(7k`Ak$^o{` zh`(gOv~VTL%#7gEvWWQs@C34C!EoU8kaQhp{eh9>OrWpwwL_tyx4C604i* zm7-Y>&Gt?{cN=6zeEU{)f-e)VwhI0%gw_sFN<8H>WP4zoC8X!p+WATu&sx*g+!q3o zV(n%H0W}vEJt&)MA~20D(!_hdDL=20jHuYq0}BMIE9lg8z^MLrdR#*zF*BhK@hazi zmKs;Zk>LtI@5wG6lT%Bkc9|3oAyF~}D7Q-kyeqWbz!VwaXy7 zNU~v)(;RJIhrb5CJy0S?%=>6#_^KkEZ@#`)$4#***x9_T9>3^~%ynr?jE^KB(g_Q6 z_eXhT-cx6M2y-G_ty4dBwaF>{`k_b7830E_ZX0VM?v}KDoCpGa9fdM5X=+xxjY?Cl zqF2DtH)x7m5UvRGR2o4h*M{fA#z|x+B4?XEtXj0zuGI0TW*hYy#$(JN<3&#jj4W{q z!Ke?q5j^gRzWpuRDWF@A z^q@v<0V|HO2!4qX%?J)r6c1IiIu8AEhmXX`aB`h)1iOD@8pe~>`=uE$~o%ls71o^PXCqn@%veVpV+Ma$;bXeLLRqwxb}jX ztc&Joi%@?1z2qU5kbpR7EdAvwMm=d_mxsT!O5{-TV4iGOK+>NdiK&E}oxCSy?6=;X z`Fs*D-O^+FSqz0URE>@Z8HLBV^Wvcz^UgyO@g;BQplaBj^k$93H9^pBBCu6u!eq#o zGU&<>+VS^2?QU|YkOat%@dVQai41)+*KM+TlNM-Lf8U3JjqE2oQ_eOAU?q_GesUv0Si=2;Mi97Lt;E=pwLNED&hXWC`apr-n|wUmU-z#QzR#7VaF4nSVJ_QJ zMhZ!LT_CuC8IG!lH{TG-EyRAB)=S||>nvMhLbe9i`9OaFL&X_o+SSrgvaQ;o@vOh)9Z! z*sK8vy$=u_6eM)pkhZ}m>OWqbxW2r2ew{UHH_9A^2?N!gOvg{Hp;6F`mqgLI;n4(~ z5}3`-409wA)OJ}Bj>WaD5g5(T#6rEloEj|HLbioUJ3q@;6KlJKA`|ljT!?Xzvde3> z?$1LW-OzyTTB!IpvE9@PJN)W>eH0rTS|#@4oG*moND1-KJrJ|EhsX<$T`bYDfBK1O$Ii z@qkg)fh2f+!Vh9GQh=h6W=P?NcZw`p;bELlZ)-CWSTg4qEj`LdMuh(hiOc9CTdRgV zVG1RfTtw=+bjR-rs+Y9*0?~0`0?RX%5+rLpYxk}IfTF_mm{aXNSpER?41`ViQpbg= zoXtu$$CCBsS^@tDQ9<2tnxz24Lt7+i$U*e5^h~CsinFy&%NNzuZO&L85AIUWguFP? zUz-1<#1##A4cW)S1JySFjmGU_CBFa@;i)>DNt(~)%FhyI%cqCZiZ*+RONu?7 zb=`g{5^m$nxwmLhk~#<2iMZo-1jOZ}Dep7_VINTLaoo}PDwiuE3YAXiMfVOi+F9OV zWp(d$B(P5k<{pQKj!$c=2as$f+PT}AI0YXGpKLuxG&d;ta;#*?J9WJ5d{n)+c)j=< zW+)Eedzm!B1Z}o@u%~nxh(K(e!ri!tVM_po{_T8?M^2lHq%t1EvH)$(og0`cv8AC% z9oziY$5$ME1yc^3J^mxx4w0wcF93YRjr@MZ<42iW>h%II!kW}YE%-DaEvDjklvC*Z z!X$U)Or0-ES1w%-nMkTFxo1>d>;L3iFLK9quC6f!Na7ZXLQ|tRuNLhKNtqdR3%+GV zhKT51`vuB8)U4ACp8nU4t3XlFss&JYz;lGd{S}&*WL1RkuG-R8BXPsjL(&yuENcN> zQ5&OwgL#cG zh^+4r3=V*A1F8B!+x51x>V?<0nGF=jYdI?FFVvX=s6|lA5hVfWe#0*>1b3&sQo%vB zg=-qIc>VR36$l&GG2uHqY=Sil;_#Ke%*5wi8jRUg=UR!%KT2Rr$C()9C*H*d==^?{ zWf+l7g?>5Sk?Tx7r&~rONprc;#~VE6=R?W0o5J$gh?4ylPEnO;W92J(>41!HvCg0$ zP`gM)V4?>K`M{(1toahOoKZ-+$eY9+v|Q%t2_;7^j%XY%Op&qHeI z>3$b>&wcYBd=xQh)~2j~;r0F`nfeU7@SjO;K#1)Vgo+sdNKlK@(NJ>USJ<~u>c@|A zhFmdXngu)k=u&-SgCLvoo=cr#j&N<3??bw69iXVIXC&j9p=3Poq)o>h=0!L|p0Xwx zk=2Ozi2QZp{eB}03VwMser2;qn0*PeeW3OLgjd~$zukGhnsK-7^FJs%$M8tEtzE~q zZFQ`UZQHhO+qTV)osMmzW1AhjgOjDb*V^B=&vj1ytEwOMzE!iv9GK7jSkoaLz>D#m z&U}FsnVRFf^d~Q}^ipkhy7y3bt@cij()H;jPohukr1p2xSydP6M`Dc_5+qQuyQ{f&p*TV_srz-?>RO35)PSyXW z*n3ny2fYR{cszYGShf6@HWi?k;i`krc^K*%yeKLh60^nh{2dx&_T17Z28|Y}Cdfcp zRNXvRkV!bP9@3*cigLv>n7>zux{1geTlVFNJ$p{TqA33&P|z_)`-&ykR$3%HEJ=Mb z`&oOR%rvs@Bgc9pkYpuO`^3^!yRpN=jS-5Z1U;9;XxTZ(PDXi;lk(&hEKbhBe}imZ z(<2Jvrq$>Hfv}bFuD2?dF$vw4v&?+8l#dBvGz5@iN4AUvl$Vd+QxdlgFL}(V)j#Kk z+Ai1wx3I4ZF|bEr%g=629`KMQu(l)&)*=?`R&+p}#K``HB1ZOs{kw0!KK1}VG|t72C+w-WxJVDG5rRh^Gz&jdI^e^driSN z3f`FcLn}f)Xzvh6*n-ugVIg<)0f`p)TG)t^Mj4@AswVLUo0x!SJAvgSB9QFW;ZU zSSJtH8@aR-EIt7f($2eyfd#H?X^al(H#xK0oJ>N zmnxxx0J|pJd8sb1^y-Cf%E_aXuM(ZqhmZ}GF3w774%;r6|M{9~@7g-Qn=llqIXqr1 ztt(%x?zBt}^TgIOdEJ)GZK3KJO?tDI57J65~D!!zoqF6;fuM{(B*$%k&6X`j3UKB8eA3byDFlQ36ZpbBf^FWt8Ue6i+V$t8FCeKqPKX3no9zk3cFSTsz@&*43#^ z;X+rzOYvAm`altntFLKa%^s1J*F10PyxTm`3pYS!TO+Mn2#sa;A}#I~6NFB3-cKoh z{?IsHDTFlj#Mkjom*d8{ss%U`R+F^KB2Mvb?tttJ5?ntW@%e-Qkb1Kb{qpSwlGxJ7 zHm@(tmp1uRqrJxkInv5zDrJLNx{)HOz@(4cTbJyK?Tk`^dx<)7VCyE+ z8{Y`r&lF8c&pY=*%;}CPxp>s6J!C6==@}l6U<%Tqhr?^wG)@21@fX<6erBui72UL1 zvS;5MwTRm>XaLqLwWCKE^WZNXK`T9!)DIFPFgvmOwl7d&E5;M9bc-4oW3P|wJ>V$h z?H4`z!q6)#9J&=oHX19%R#7LXZwZ%ewfSw^4_VaYw>hYJwcQWF z(5P@%`B8=SIENc>!U94ne7P`K?0&vba#er=Hq~F<98!|%s|hWc3tln7*Aql|c=k z3B9u14m}CI%7Gu`EwZ|B6~55uvV>`11Q(LN8y$sfC;)#+#CFLJ21TEDFn8|dKG{lO zx)U=FamzGfsN2luXoTp8znsN^W^Rr-vZeTm?^rx|%_mR@ zTYX>wus^xOD$)~GUT;yJD;n-yX?Eh&0}Pel13*htk`*wzr;QxGIy=EJ@g$)m+LH{IVrDL_tF*U(W_d@Z$g`=-QvMDWDogD@ z><71OiwXtv&JUZH7>HU2(9|t*me#nsi+iI=P6><^w9VW3@r4EtL~u%`2?k&xos=>L zL{1OBr?Ku5Q69|2^@YMM$^qQOs7nP zVNVPyso4P`?n$x%&oyh=U;Jb{%(rxhmuKx;uyPmDBOeSrPTX(&c6ZdZJDcZ|?so4U zHr4pR^;HV&l}jo#OEWQMX1Dc^-x!mtp5s@{Z*KfMJ~#mJkiuCn{idq5(6df$68j4gMrtx6DiW?0UQ~z;D7ZS8fQmgyL zlS0pEmQoo>BPlf7;bW4_1wAve=Z-FhzLW-CDEE5CPQC0x?ucL(3YO?PI;l_qeUyhR zr|>=Jn(#wmNzNM3|1umDkSQrzaE|MzWPJb&!^76r!T`uD;FaIFx0FN6-DO} zt1YcKssfDT2MEfH^B#CdZ17J+B48|#Gj}WIx$2rAU%H8rLLv5a7@A>_;Ty+HQ?-Es zxaSl@M#z#6Pm8Qy*sQ5e!TzhiK0LNJ#|u2(UgP?aPF+aHCG?W2_rLm$O!F1r0CFUJ zE|Epy9?;ndG$0ATKVewvk(oEL5)w#t0El@+k)5n=p+Uu)bvtJ=`5WbOXs!BCf(iTM zUpyJPw~>UFRK4BQ-q@EDk93Nko?q!ZN!reNhV3^FkPk+t2pIo-UR<3n-H1f|u(spf$a`TYH&whbuH8Ig?kfzs-Je6fM{m zursf=BtK*wqO&<$3wcjPVt)hDGCG-m?>Drk=A3;6HvTf$bD3K8aX)$~q`KGKdq6n# z)8woC8QjXDa*Q*dvaBVXuTTp^3^TPFjm1gWCOIdoLI=rjR0z4px43e#Br3A4Ke9&Y zQ>Y#bI+%?F3@9O{nqL)i=TbfvqAQc%Wh~GZmQ(Rud`=I|jhV&BX4y`%PKs^OO(+0R zSEM)zB91+9(JKou;6yB&^{9x;ZGHLal(&& zp-OtwWXzSg6Oie!K&)uU%H3KzG1d)dG1K*`P8%MmJE}S+mvBrO1I2fhA8hcCA*f7Y zh8sInAXw)9*m#zjjgv;_xo7>DBZu-T1cFM9dAdInMapqhK`i~tI}mWrx$n8OI=LHH zQuLP5JW7lCsfiYzeXrVEuGw0Af8p3AIReH1Jh{~88MA%hw=4kazow5Vc;SdVKBeZ- zR4?!NsZOz!H!xK*I(;gMVaKHKY$4kt{QifE*<%(J)waU614=5_U^O0gl&kscm69n1 zCp<%xwi$Y2Oy`0#9wg-H;iG7B<;tty3dngd%Lb5}@3P%6V=KQF;zv_SR=8(Flj#xg z0_&*)OPVLx$_TaG-oj8{SpO1);P zcx>vQUBdG`bm2sU)Xd_U7-jl?GR*M$T}&+LG!W*E`?@aR#(Fc+$_cPdNr6Xt6uxx; z2LSYB{xlHmaKOlG^Ew`YN~4r*fg0x%@@j{zcliv=0qh>czm#> z`8%L#Pz z2c!-}R#!^27Yr~};2nqqE5jQ(InzS{IEcG!DAqYGWTmPInF0rE)sUGFFk53Nl)yV?>$2v^MIXwHx{e1Hq%I zK7!cB^a6jm|3Den_JeCfxI0FTe!yVx(Q|hlE%v*SD<&NK27R7RQD(QV?Ka0agq&|z z*JIkRm+-SiTRwu}Za|HU7Otjmizt4>;0Ksy!Nzg|X^iL3z%mitpv~f--O;8Rmt8IS z7b)U@$Y}-_wrnYC!%SRXPz1Iz_unE>ML(#|=V8E!x!lH-QyjlL7!>HDz`lhC!}9JN zd}HV;j#vjP6|o}$K>UsTCk_rp2SHxDu=<3;CME&2FIZhlj$SRL&rO<^Qu_dR*v|a%4bW#&F2kARb)C?UE=+MNs*5p#dj1{!LHK1AxD`D=w?kQm z2hZX`->|u3QzATwg>7%!vRcas-V-ZRu2O5Fzh_cP`VSDJ<_Ft1h1EeN&s)&D*!U8S zZIpSp!@h>kVDuZes3(XYlbigRpWbQOvJ45eNuA?pu$J3Wky(?m1e-mRJ)G}ck#`qp z(muu4AxiSVtI0;coXZB+9J5ta5|Su7WJyN|@R{|hK*#haz^si&i5>oBd2CDLQ_tsY zRcq2&WY*?F=8T&G`c&ql=&EF_fuyH(tbtX)-+0uw(fRg8pfaq~YhqV);Q7Rd;ryK{ zL!6!q3x7Hrr-pF3Sb_zmi8J(^?nlP(llgiIyAAS|wZ157>nhNW(aHghT~pJ~F&CZi zFjO#oix)Fp+l@iG?n?YEH5#KP#-by5*4cQ>v+ATVdo&RV&po;teh6`H;@ar)mrGbMfLFrO za$=nNmh4|8gl4m-xna=m?12ch(}1ng6FAv!1^F~O6)+89@}dnrJpy!sQSmybsztzr zzHw4yCLls@Yts+tR8Stk`HvOFrQ`*F*vLRtgd!Z>O3n>VqLN>?I$mIy%lp zmDZ*bfn!hq-&C;WU*0&Pg zWYF0o53}T}^s^GFxd2!)d~N|ilR*BUQfU~GRWSJIvI*P)BSKWb6ba%JkbITygG0!m zxG*g?p5g$S#^6mK4-@+>2nE@RUgKU-R+n<~OK}wrHu&B;Yq`~}LU;9a zv?5ALTkOMy@yc<^4Bk_mVjJm)qUo|4XnET`BS@!O0lG>+Is$!$-zdkvkk8V}5zC>q zAy;5Sc?~(t$Cc(0AZ|jNgQwLoa+CP53~oXvZ`1!Nn;hgd+iJ=S63aiRy#Ur@Bjm~G zYIwgTPK2q1%`-fUY5W@u@l)K_332ScT}^6v zGcn~4@0YIEK+ah_TJ>dtXf{?s=USCIM9 zLbxhk$WS)T#{dnVPJa+YtAA;XO~a9UAh&NDfVJ5ds=-g7dEA*SxAwq`ANe^WzzOuv z3~ljL_(XkPeHr}{%q&FDADq_tJF8IE^JhP;4Qp@XZaq}C$rS3jsOy9S%VDVhk%!UA z>PRzt55>V*p(xDUBu3erz0Ra-k?|CZ$~=Kd!egVl#x$sLWzb4+!0Okh5@1xhq8Qv@ z!kqfIxD+|-XL1kR?Zd-#YUUV>**{=OH3?l4Lc z>eBmOlvp+AqeY{W)XH z(agV~je-(JLy_dq=;qaV3{%R^U1z!6ss5dBp8F!5AJ(MgbHP*4uiFt2dMvkN?#;pt z6@A{8z_A%{_CZo0cVB4PAExFq*Tu~nu>OX$EPL}mYM8$R2e^2JKfr_}{Mx-0lQSa-4 zUjxVqeAsVQ;PM7hfiZHA6oq~ZHM8su zMz%8f?1^R)jhwO3GIz{1(0wYqKDLFP8!b(+$jG4?%&&YENUb1o{ z&<-I8dAiyYS3%NiD}ru_m5Q?fA)e@;CrwY&V%?r2xaTR6`W?}+zLv^U(2d17H3I<06;UX8u|uei~Yjr$pJnIbDP zdwOV?0Dc1xR9d~Q{<+W?(p375?d%}W=56|ePUw?(3G)>nT8{_*>-2-_lTl!O~eOjSST0F5d6P!q6Vo4{*a~^-*-#O10d{zOXWg>xb-< z^_U*n>Pjp9=1!iF+hXGdcnCtO>H%laCIX#(fXO+w0gY$%I_}1cXG8*NMVN5*rIdx# zb5d9#R7As+O$42?MTr^FEp)%d7U+LtbpNE@?LqUT$a!scG99X_m=Yiui1#&mhR@f26)yXw*Yc*h=HTGW)HDXmA(r2NF_^*G}fQ`xUv&#IAOU88(cG{@^_ zW&Ma(ak`c|Rlq%hZ(*Q?zOocpSom*1?)3TEqdst_9Dv3EdZ} zbn+sGCW+uc`B$@C`I`GYkOVk1q)kLFtHC@boA0C)&cPhBB%R7!+4(;G>!^k0*8(?xzbhU#7QlgD|U=FXu zQeJml+y_jkuEc}^0`sq-nfveA#Jk3~tni`6=DBC2uMN^ShO9&T8x+Nc{@Mcc2&aei zi944^6?+;DWl37olGwCk;7c$}s9vk~F!=VX!uPjE%5^5HeO0s9DOq(-3qlV@;*ssS z`w~fMJUF3G2}yMfsc}u5QVw3yC{w>^k4#mG8E zo{F&lJpA}Sz`fFI9Jx!?cn;%Zc$H{nmC}j$uyA8NOIGWE{;4kFVX|!R4VTY}@c9ME zjdfy&PfPdFFSpDmL>h(Wh96tQ(Z|CO^$g^6mt`u#I0`xu)@#Jg-vKfSzkk95ax)5M zg^+k{_O_-sPtO~pFn#(BUuW-kd-xYm>-rW+=7K`(-=)?sa7Lk`p;~T|iKeeqQ}IvL zqSmVPN1w5SSE4(*l;>5+pcPnlqfR)VeG2r>q%!KGZ}z&4PpXRlqSO9oH1Mr0$fgJf z>6;{&=-CZI7byAKTHtV(^23d7nM-9kd|O_SWV&BWgKBCugsRnaZsI*+1@%Na;<>!8 zA!B3x7R#|_?|JuZtpz{b1;sC57tNrWexQC`)uebdd+fka&kOg7V#1)m|@40llx4v#WhvR_qv`7R<;|6!&a7<$}h8SIb57 zshGFEyOJ*IIPi8LorEy|O_zmpE$7%4UbNr`FQ}X8{1coW8tfp;#Y{#)jf8>* z3H16h`%vV_=FYvlnc{06?c1-@gLqwF6JpRy5&9fWVvq!Ef{B{}dfoc!2!gqt-+ z_lNd0?d#u4NgVsLSm)oaCj)ZVw&Eby{tnSy5TNPoJIv|j0F8_uRFQd14Xi9jLqQD7 z!(Aqyg_)$PuY1WW@jKnHBVCG3ciRp$HCm_i(zhGB?mvlEy(q*dm_JO$pHB z)(c(lk)~e7w<_=}&qE-`jFQf}?T4X0Z5EtLw2%@jKcJ%^7k=p@f>>w?4%q1PRfgx- z+2y`XLr_Oiw}dEtH@G?&X>74a?h4diP0rsEA${yc2*Rn5&Kt^b_23#f;$6r@=epPA z>B`n$G|Rmh9lQ0pz=KTw*38a(I2(qh)Cd*Q!SrX_`wx^dT{g6)Sj=!dVC^_8gPnbo za+`3YNso|CWwIQ-7<^lWe=NH6vEr$5_aCPL)1ZU#-JvVyHMio)znwp!o{@C$wK}6 zb_pP8gc0r%1ICf^r5gTprZV^=!Em%V!KM2U=+!_9=uMXD?mA3UIEKC{_A3Jx! z$i`3#jQBXJ1l1J;rU6p$vsUK8tB89p@@DhUAozjy{(Z)xtmYkDFrjRKHWEFXnl{yS zA$qaxvz6ynFiz9Bjhfn712YomhpH5af3q_9WY}*mq`Y`VZQDt`a}8ge$|0!O2kQX(Yb1LvOFTbIseX zY<#5V_Fb_M-_LS{PC5*ODS=0asu0?I+N5#82~^L2{sOG5Hi`}|V|$p^U7rQ=trAWh z0VfMRXtFv3Y6iMJ!(9xn1u^QtjScIlADY2{CQrLc{7=lwHBR*Ka@D1<)BT@cynObD zJ|I+WuZ(W`%=Ee=!$yS2T6$=Rpmx2}TM=ZP$w;>)p5m#Hu;KcsPS)s0h5koi-?(wF zzy8bhjT7^>7yz*{KiiUix^KKZ&)8(xz92C62|AqeTVexnK0wK8D<{E>&O7Gr( z(YS2k;$>wh#hn8gzHmR1%(=c^x>Gg`S&n)D|8iiil!5!vis8Z{3;1PY8S>xlu>Xpc z?C;b;ERs=knAwSfyll{Y}MHk5zE{)y!Eg*QdK9`4X zwC~wXJ{ot#>pQ-DwrNO`X48Sff|e6A8sG(D%3AWgOo5w@pG_4gP$q{KH|D*9010G@ z#XsF6_Jw?(gmWi|3eHfJV`?Qe;MR$}ekErMK!4SA2?s|XGthoB3G~a{C*m1U;c2!{fKvu+B^viEwRt>2- zwxIRu3?~+tmS$Z@M~!n*$jkVZXp-e_Vdv8e;kSckXitk+Ki^y{el&Vh4;wq@nP16l zqvcNqolF0ZyZ>M4?DoNBg?1rdzIq6((iN!GO_9wgkv5N|w*)lFCW^w*jrHCD>{@v2 zX9ke%X5?hZhTpf8pE29vM=&uAV?^tT!fDN0idKo+@7Bq!sm4W9NBZYUr6~6?)=J17 zwJ$xC^}4P*)h$)7`<47|W$}a9)N)mSH?0}lAq$;`_Oj~kgie_9YA#<}%kr#Q75RsY z7u0>ie=FnuXdzK==-;&-q7RZ8J{+2#Tv*dzEEu;f!`j=5w~jM71RXkX_6L)BGqhz# zq!soFlNYs8-5E8~)aH1sBd_8;zBtvciWa|xK$LKqhImp!^pR~gQ5+3IM=MCAd^&Il zKjJDN3)7K!f7=z`j}FneBi1HW9=DW4S}m|Z$j1#nsv`N=)e2;vxLp%dIho`pL3_bF zJBXZZERv*RYhWaR_|#3l{7X-I%elS{3_D+vW z!rV)KJ8i{6d=0N(2QiMN#ZiI5uVK5@gG*=+^S#4mHmUM8?2EuG)Nh>s*`8qp73^&b11-=7&GP-+iV`R#gX#C)N|5W$( zLK4(5!Dxv9U%Zz%k^SWX^%-(JO$#VGkiHIM-036M4)5ORcnf52$ z-yk#ztMU=XL_3eTlX8+V-$QneG#b~HzyQKsN4}s#-RYi@@n&%09Cv{1*A96TSi{A81oe+%XrfLQ6MFmd341HrLrCyCR zc~^`Gj;hF5F)Cl-A`y5*Yg*k?QcMxXR-T@$BN4fBRpD0ME08fXCnwsUjG>%65=XMt7$=L1j%_>X^(gaA?cr#^%SxHQp-lbyE;7*O;x(tr$i*_Y1vd_onLPA}&X@eeB(=Oa6{0TDd_qoZV^G%kV$696J8vaA2st5( zL`kof)SSzlrcje1P$R*BUel#FBBH!s&MP>U?S?tun^Ce_l-*q|Dq`|N28y@FnLw7! z>wRk>4`m+!NuR#LDu*u>QxDo7R+S8FPK9EelbDI(kRPKFZ*TzTO(z;W_&&C77O`W( z>;vnKUpS^2exo~b#CY&zrYIy|h4w^& z7Ue-l>#Sz6_tE)-E|d|B@S&@&4vqDsU8BgS^t)a~Qh^uGqFsEv&p3_Xy!n+GAxbzv zX}482oFLWD^%5I=eOs0yAbXzJAE?#UlH|$7Uk3@lOnKe;R5bUs3Od)#`+q?OfGBxG ztiZ1BsrXEvYi|oEO@p|tW$ASV>?^Ulqz1isnlvU^7w&*+9Q0I#*ry>-R( z=@*fGDa!I|#oA+0@;Lw@EeGRUWr+GG*g>4RLNb0XFzk7iRn07(0!6#D@>zltqn|RzCCq6w=YtIwGE$HlNkwTW(he#w}d_p+i3*EG+cKr z9fcCmN$)q%`9NXtYieaa$+r2WblJ)u_5yX?nzsQs&1)i47x($;Y)qAg2z*kQgQ;ZN zG|Q4>Mr0S=6SI2k<0{;QLF^_D2%@ZnT!O&4Sx7(oajg&(Ayk>9f<|yrvFa0~lVe zgUPprQOub^nUTixBZ?jXX!(z@Fqr)908T`3%qP%Wn@ips7c9e{grxur&Oe%qXs)*Y zN8fSDNq>*Y&u4> zAVmNx`qR?L>R2tf?Fk{$nYhStpuNoerQRJ-!WZrnqvYkA!A#lQ-s^;t4=#$qDogo| z1ad5K1WNJA@%X%O?{zj`xoJ}J@OEsYAv0{QdB5X5wAvXF6nPh(tP%sKD}&L=E_s@P zFTme}p;RV5!pzRn{=EDtZh-5N|IH{Vd%fz`5N%04Bb_1^A!HDNYY{h;?ce-x(4JDN zS058Fw4WSh#mw@jhtQ5yV`Wod4Mgnxc76_&3DUbJ>~7#8xFCZ3yD}kVf=xDmcxINI zg#jgX$R+$?C1^Pa_I}abq#GnpZdtv-+eXy$XmHj zmQzjmn?7!yH!l)C`JT?dWdUTw6_-Gp|K8j??W0qxi{SgQL4&gOFK2^t1s>BlUQrT?_U+c>bHG~Vz}{mbrPNl#HBNeD+-YOID- zq-uo{KC3mS9f)6Q)xl90wtwop`MDCn$Ya+uL+E{^YyR08=>l{J>z;{I%5rfg*ovbl z$)P$x!n9=A%~2TXV$&g|D5w0D0}*9lzw>YNGx9f;R{A|+`$n*Rk1M%aDtbsWoTS2a z6oq~q9daoW==y(FXrF%}SHQWk@d_nrfzL9UiUEFl zUWnlDhbl6WlN?ASgU=`w`T|Q7GXiu;oS$d_d6Vd>18nZq$He3`Ol?$AB&6*v8olJu z_PwvfDS8Q&-KY5yVM+iVt1C*4zKIr9k5O`={BG|y4^DXyQ!hDg5E;*;_2ph{mo+v{ zMp$c0vhQ8)9gzmp2+Lno289gB#CRBMh#T;^;)w|FP0fJ=B|ZJE%icqxHaT>QWYN=L zu@9BUbKVcgb)2TAcn(RLCsL0@r5%RV5OTyAGlJq2WvTYzn_B7Pd2xfO z!LxoH@bO2LqiEV`7iN@tV%|j*8%0n%!s&(f4|4P;@K78H0l%9vj5xKY6gSh93bt>> zT3L4(EsVO}0&xzAXB@*>P^?_!P)-rVA?1y#FpfiZjisPk)$0S3Os`1M9LjqqRn#(w z=q&LWD3&$0x}~z;dkaH0vrcW)qTRh;R%&==q?Aj0Sv{8^uODLA=F0V*Q=xWse)Z$t zHm%e-bR$-iBBbvxc7YZel^TOsjRZY;t6Oo-2R=Tc;nSd~-I>HRZ5u@A_YkA2f zk>X^nNOrItxGTuka!iJ{+*=p3a~rw84G`y0d%TFZm8w0-^x4%Owz{XuP?$WQRNB`r#g+8W2If1tm02784Z7=uD@a_R_3(KL!wQF^{CrDDYiNzh1}ANQKBawmzxVB(>a*c zTR^jGWMZD46wLL@;8*Cw*n`SF!q$7f6iz{*yA5p*bM;#|X@)}f=q?}k(Cc4_KTbHw zvHYLUpLs@7C%I^`>-Ph!Tv+!je1s4&RYYTa^!hAdp1mo8cD&{?Q2^ZPmIu;rMH>q1 zV5X@KY|V&*_;rNKgTwoF$HctQIspR4XsLa?c7cNw0L_GmwhcRDm~at9ex#|4dA~d^ zh$1JlUEl?fvP!`dLotKUb`1i{t#^;8eigvIMiXVXG)SB{F(7~Jfp^|4m_rNM45CF7 zL=i(R4JNcHcLz~g&dF7|-Ruzuq%B>s8r2kn60V}NkniHG^yIkcgTkyZ|@S_F&+L!7DBDF&A#+jfpstiq92 z(SdDjR%V4SXVrYsD5J-$F*0@Xk0EiO5h9^>()3ogVj2<3xOj#l2;R|^3Lk(I!Df4b zwzn+&c5(wzlL3x~%Lm93jf6BtuN}mM(g~!7egV~0*s2Y!J|=~^j3BUk{n{g1Vb*lHjqYO( zWp$YW6*6NnHTYUO-Q04MZmZOB-H7e2WOGhMoPQo#he6?`YRQm(=oNATythzalT3z= zfAkvHM+3w^&IZaQ-|Sq@Li)j%eg%Nsu1Z9O8^GY+cHgYHj(z`4qEHHI@%Y6=!^~(n zf(I!FJCkBr-iwlw)ZY?;ba_;WpL5N)R+tXdJxYS)#CRAdZ|kST1Sw}`0)bmVN?dBy zDeUrA(B1O#D?Fz^wPESSv99y8#Q=68jPupU0pB{Pa}5;~-ld3U!q5ZpRERPVCu(bP zz>oHki*``2v8YQfN*Tj9G2iS}eH5tW9GB^qLg#r}Dn0b~;C)@FnRht0xHdfI<-@bg zj55oklI+*6O^;mJ((1mshcsTC$zyb+jvd)8uEAn>mUT`xRiNPmL#)KozI%LyYH>$; zV9H!P#UxWNF3-!$a>UG>6FsJ@n&=LIT5Y0%w0q7HgGLO7LTf+`gt-Gxa~up?c!+aR zD>%i$`6^vT(5{k5cyqf+#wfsoOiIS*GUx)0$+!7VDNHjgdgbW$6AMn<__|83JpF)? zt9`cN#RIW##*+4_P!L~HV4x>q$U20#t)6k(Rovb6EGaxJZpyy^n|?ULPDW%SWs^|x zwsJcEFa%S2n!xZjO{W*OzNJ%k$?)=7E4=4N$wZ?&`g!T5-p<@VHCoch2G-!_qA+ll8LsAxvd(S`z*JrR2x2aZL*u+1Uk=-F$4(xa3kr0K5i{UAJqv#9nF zktgK>6rFJ%p!}MPp#vLGwmR09l&FGOu91fjSiQ4Zwrf0J;k5lE*aASPcrxI`* z@e#=9(a)hwqnc#SZ4LeePBIEVHf?_0YE?$@az`arT&URQJjUNR`G(q}lFO5u`rdJ(4-X?o5y&8M zj|Uy}zEj7Nl6R+5bLsh&V11gc`TT`ec4kd7l^L6{Z}1v-(bvf z@UfZfv(bh+2e_|r7?Sz0jZ{_OJ7|k$SLTETmMG3<=NRYw`;eTi7yFO87VFu!jixfMnY2tW6I)1lOWCt-y#cDzoA>tm6wGwhl2yp7@f+b4@ z_@amhK~xFi96@OMi3;@?jajs*8^Ztqr2gvi6G-n1 zxE8WCnP`y&*0e70Pu+^a)FPH`lznzwc%+RDE$EK_;VJEajMP*$Aq9^Tg z6DhS5`6V5b8Zc{MLhNxzE&0-~*`V8Ea{?P?K71#gwSaJ$^K;U00MMTDj=rwECOs^A2agC;TNMJ{KhALLw;+sGEN!dv z0#+fm#{(FupI65XOaB!AD3Iab1N}Ji~JB{*7Hb3+*uLFp0i6 z6E+cM(~jkWECrzu959l^QITjCaTmy{2jP}Oj(lL60GWA9{}v6=M{BSH4#7nA+~K$fDn z)M$C!EeO~^rqOU)IlGaNMaa{IXEwA`>^Q$7(6eRC&I@;6-wv*Sg@I1yHI#bb=u_oD zlG2cID(sl-`Sgj*Sk^AD{e*kRk4}iXUove4TzP#YkSF=QZ9)dYAA0WUJI4Q*kqvIL9np-5zUYeQ*PwZZpdD$k1di)1n_P zOe}+qG>k9gs+c>?6?F;eptgb#=f*JLamIUbz)jN?(WImOXm-;R^d3Y9YE?b0I^Q3K z=|#N4*Y4&#wJ;CCeIj}(S;DXjnx@rfX%jkJPJBo>Oo!PHBeNRD498RKhby}k6p>mN zQ?$h*;E;~>nIm@#J6cj_c!^vSr65p44o+Fs_@`Y7VsY}}zT4O_Y1ri z`@uYDosh5&0SMx{lM1J}>yl+h8#9+U_kTD$2PIL0C4r7@+qR86wr$(CZQHhO+qP%! z*!JGH@%9I{KcKs!D)O>%|vbubtxod1i)TtR47aZ2cio{-VPUj;h z(`>_Owli~DQsR`4PD8c{OlXUpI~yM6gRRRt=d5Cl8#dqI!YeE*pVUk10rP|dss8WC zOK7&(HM)^BN)-c-T+!y8POP+{OW->;FCWqC%bK zhdrr~)~YwxW5ukeYC2>GPO0tjnbkfyrF4IHB-V}C=Vg6ciOGbse(C|&68+<}6V`M<2COrPBdBSGvTOpD+))$H8zGo#`-Gu@hp7&?|fzbg_1_@QrF* zA>QA+!t2q1zW6-N7jIGP@A<}y#i@8b9ESd;6WBvEdniTMcrJCeq{Q}jwVpc_+w_(Q zh~A|6=Pp{2q+OkB3)<+LW3SaEN=3Yk)UQPQzG>ET*4cU31QZ*@173G_*)AJU0>a=W zA)&T0WuS?l=Wj64HZXU=X_mw14jEScIXk^X@Co-Ps^qp4XiU{03ciYRqU&5wXYPB^ zr-Qji6Ef%l00EtQULbU5u5;;t__idRVd7h9mbr#PdV zGZs?Ei{%gD7yJLZ78ME6>=f-#61e^epXul%gf<75MpVJDe2o<<)O1mdb)gLOIIB{_ z7OUlp#zHJYMpQ=@iAYY9v#X*DL{n^G0brsRMj~^%StS19J%f&x8D^hK7ns9WXtq31 zHg_m|V5NB~_BYKrdMCRB!O3pCS*q&5OXoJ)7SjbV9wXJ-G$2xv;8h%`0Pc28KB3VI z7HmB@_s&(tv-Z9IR@YXhyuU(7B;I=w4w62(`GJAwCnok>=r0>EB#$yrNSYRIy%(v@ zt42rEl$mWE9d$L6=+XiW8s4!pEu*}q;A%M)Ksx}JGSJ?nN8))`>&Ad+7PRO6gDAEw?9x8?eZ5>}0#t1oy^HGf~IcqJ-Od_6I%@W=1kHHE;} zm!*17L7fP&HgB{@5tULky<~|_g>K;&Ayls4;Hu<=O48c5EshPOlD2T@1w*2s6E0ws zs`j?$4KVQA9f@zV{h14%rj!s&ETdOf3Ya|>Qyc%C_FuwbnX5*#ahEmRu~2~P#lY)c^0??_CXS0STA{~L7T1mBNsWP&mt9aRwb0bZ^8|? z4nDEIlNK9)GFW%9#10)$MeKVapmTIKp&!)d2sHw8*o*Vj_yVQm=J3zXM@Lz}A4j z5qA)b9%K3SV*|{W_f>UJ(k5aR1l}w2j0pIYGKN~H8(hKlWd37Es?8r~uRXCOaj1@W zth+N3A_gBm-kHDdo}H?eQ_R9#=)jw40z~@99OuQN{3~t)7Ybzk{?Y?bOXXHI1Xq6y zH0e_xO+4~>~4$pV4S$kE7@@(-SMw$q0v>WU_4vXt`e4Qdn@Y=4^dGO?3y zHJn=sc98C%G)Pz-4|`_`V8y?5vHhZ ziBy(z98~1{Xa`QO1nxFG?@vcWHsaOwZMKm0#kFgqy^?}iL61Ag+2_6k?NEbqpyqJ0 zzOIGvZ8lcz4-Fk$!ATc;hl)SF6lbQR4)*9{?EljyhVP#w+-O{w(ngBm#!X#}F|_+5 z6e;@D@EJ0yt#QeMA)?%m&r57)!A#u$>!z&{VIE=ZZiFCKhydw*H#i;Fc$ zj^CchSlLG4-aNkpCAGT!WHgUR@w9SFJ&BgzDaA*m2hU#8t8q#W-#)Glc)&k1%2Us@ zei6)RUVGAohx`2QP8`t%5~fFe_BHG5zOTB4$Kt-eb%-&CeOOSPaba4@{BbHfXAzOa z`zJIJ)r-Di!8+s$;`Nn8RgG}O$uu>Qa}{+&mqD?UC797<>Emj5bYYfR?%>3yUv31R z`73ztscbAdNDg@V#&xR)wtlCF)i7zyBf2#rtJ7p>pavI6F0`(Zxor{58=DH6t_1*k z7g00d-{6eOFg{aSqf3A5j12T`gSXuu0PxAK?l!l$oPVvMt+y)%rn7a zgL2hC@$XC^n;aEit^L5OxTEZ@H$xI$BeK8;`8zPseO$#HRH=dCUfC*@l@K4Lt;_9` z`wJ}nq1g(nLbE$I{xq$o>-#B;qUE?R59BG^C;LW_P~rB6joOWxkh#zJIRfHIv^Y}; zYRVKug>hfLtv6cxDC!l%CllUMWmYlxavL@{?@y*Z4IJW52tn>OXaqG8W+aV1)`AC; zq6+$Vm)dYq7>|C0&$?20jnRZjmA0u1yp?qm*wsYG^~Tt3BIBR@gF=Czk+LpSh04)1 zWA0qqFutpw6|)FFZg2zwPl2ktIECjJ$g^HO^wMnFDym1RV9uE36(d4ng)b?MylT^x z;ofCPvYuJVT#*4`Yv;XXx38sE>KLkcA!4My5~Bpz@*LmwY6D zB6n=8YSGg@Ko2)?=ph_AkuY-`&O?pKtKAueE$*hTjJyP5T}tHj*$b1Ylh}N|UgQ){ zp$US|t0cclCpK0FZI1JIDZoIt(}(Sq7G!CJ+ZJ-`ky;Df-i&vAS6= zC3l~$=^%QVEsSzdZVFZ0uxhN6-0d@q?wi_Z`%Gpr;^ z1VbwJqKc`f%Zs6;-lZpj3ZIH_5k^H>nvXcY%k)RH`WtVk39D=2I|Dh@oW5+C_;UBy z3$tC~@7k1aUjxd8TZoXtaROc!FR~TZZso~RS*VQYjz7@`j+cd^pE8yxX*;Van&~^2 zoDc{7MGUXR>rIhWHjH)zDY034oONV4$`_b}kMpm{!zW=G1I zDgfF?+9hG zYgp2cHbNI&Gh)wAp^ivoBT)a0D$r`PAsND=Mlo~!Yu0^ZqenpF*dwSjnI>Lr8mi! zR?G=9%>7pZ82Y5Qa@(Fm1rTS$tTBwO9jvgDDp?WEWRNaZlj2v`$RcR6W$sX-)-)2Y zP;C`S;0u@2ViFao=-=%x)W01OZK0klT4R~Ws;Gj zBNhZ%;v(e=h9;OwtiNKQf8amI>!J}32v%hW0--ygUC3GaWUU^FrSSy#gGkuY@|*ut z$o^jsSxq26d!}_k5U|#-?Ek$1FcBba4EVDO-XBvO@!VuGZ)j&7lTvoZ+Pb?-A}4#< zGYSJf$6+kiR05Q_f*E}dJyWNsSw?p+BCJOAQQitODhM}|asLkMi)~%$r;Xx?Z$#Vf zGGHiRtP)|*H|2eV-iKxA8gykzj1}z7!**~G5rO3X!Aj#CsDXTDD9Oenc zA9+0uSm)`0iPJh`BQtydVUw^$ZX3z}zvBc%%o~x{>(%^1oCe|C(wyQ;TUuJyP>5t! z+;L{^FuQmGwqNC-l_h#<-R|$YlNf%aVV8-Y@uZH(uj67fyKQeI=A7UEC1RMwME)Kd zf;f6!%Id6pDMlF)=TgwVD2sJ;%Haz_N9}r{zH8QfqU7FpuSZY`(T`R1ee{__zXK%1 zO20njJOoTY?5`kR-O#`px$VJjF`U%`pnujLQH3Av6QD&N_I+Nn?C#qRmlhU>KH2=2 zPg9C#sif-lQkXj?T>tTWGe-W>c`=*;6yFfkD|6)_!vU8FBoJdylKSY{5Zz;YO1S2x zCf4fggn=#vxCKaVl$)xMu&*OP?#BC2K91jdExIxXDVPSU5IuhoZfC$Z+1-ph)RuNk zAzpWkJubve-XDcB5P;734Kq+D%7FYbR0e&Ryz#gOLE%yV^FRnw7QWM@$FR;Bwc-zH z;>iDB2=plL0~(g35}1%~1$t}kky$@4G2Pb=8{3FF5m@W`_u{1Oh+gwwWxY>sjMR44 zolShA>`r|q9({D@{)4UF1BbkFU>#fU{Oh)(3P(_+x*V_J93_fHCIKq%sEPxanYIm7 z%)KPRGXK%#+%6Lpfhg7;#Y6YV;z+lep|qAu9(k~Xv6Uyd!`X?RN& zIdGqKYDu7hikZ7yUk@vn7D`zx5HsloCi9*D`Fp(4wT^MbZfSXrE2?51@MC5llyCEo zc1W-`9@~IrNUQpQFkrq)4A0K=Y4wRFwj!O&><87~%quU$0NMv!65f9Zh|HB~=2~Pe z-)XSU2GkvFe2RPI@t5F|PQ4MB9pajQmH*y}QxXGE^!6-i98}8>ij~$zj4Or6U=|1) zZQKo+hyhuBRw+YxCb>sIHJ$+i`o2GOq{V|SmW4Uk zm0x!)&5RM564Rl)p)#SDtXagC(m7ecO&~ZF=k9xUmF9P5)SSlR8EhIBvK;o>l2S_u3nkYfgp}aK^)i9Yt2NVdsN-~gkp}Y{fiiS732c_t}U@o+d_zLEYk0OytF$K$; z1o3GsKa%(*(hDZ_akM1Ic(VZoS=W#`s0=|C#t`EdxvhsspA7cyJ`C)dQ=Du_lKS*I z@>qIgbH!0^R1d(K@r&6!S+T0zKc|D08d32~|8*GXEWjz~yGNboFIsL&FR;hxDTPE+ z(i1!jvhD;PrW^qg_Uih(C4iVG#~1&n`5<)g8X1D`pfe$cBzqo=6LHl5lCxF~n%5(R zhUWN-EQ36xV4M?g0?zF%ltAgjnFG6VJziP}NC=V{?jHup9Vk(J4`8L>>ZF}cGAqaD z^?R-*MJqS_Wydue1<@O;(4W9|*lXr<_CtzpNX_$0IYUtWBN6S$|IR!RrujiBw8eLc z(+uNksz-|dlthVkps4{qvJQVMOTQy)0USymZt%M=RV#i@v_hP~Tv`&LvTuRnEr?4r z)TMtyc&^CMwsHLG_*-eCh)sETjWuPov%81Mz4LE>zHB;Y7FA}R6olE*!}vSC9Nj{7 zqiLe+=*_=|KNDDzf1KZ_(u;ee&si)GLDK8Fp6LS~G9J67QO57f3{QB~5Q}R?5Ce_F z{Xb3nOa_qUsv1>J4o{|Y48H(b3Y^?#bq;xQ?yfN7v>lm%BlbuTj{$GpzuRpFar%&m~pHB{ACoE}zw3Jet}|KABAwG{s6C88|bX>#;;%7Vj)l>8mu`Xoy@qizpBo0UWo*-PZtBZQ@Y=mOdl zdnj!NgrND%XgwYNfn+zo(<4xg0j`16tmt{}u|tOiltcC5$2N~!MY@2%B&*v%W19am zgq;3-J8!uTY?hk`zCFO_FvveTYxl~D>g2k%EsK(&QM$kb%#~G>bn22Dwc(a>yG_= z_gNUX|0p;rv95ULwD$KHT@?ju4VBh~YJclOrJvsYW3J2<68#;HbJHNm_=Hzkq7BH$ zl6~12t;zNYS}Ta*iH?|X)_Ks8xBhC0C{E5=y`TCzaRE`uuZ zpB#CKkBG_>UI3mmu$ylt#Bc1T=N#W$S}=dtHXsnBx?5ZyZ9V~ZTgei{A9?-1rXgb{ zJs)NtGw@t&!3)(u)i9?`gUAagCm|mpSZSmxN5C|tAUc(G1M*?fawBUc1Dt`B7QL);v`B_UW`8afHvkXt47H$dL5@HGsn z>Q3OO)(=!78c8wSyy~vF%{z1(S)8eg?cchw{)(<6%!<1nx|xgAB)&?G9~O}fECLWDyu=Rs<^5@XCJ57Vkc4Idz)?*S#nMGM^BGfP*`HhKZDWG zaAg8?NG;?2#Yt8Fz=`FvNK@#>J>!Wc$#t_L-)x&dX5QF~ec*OMfw#|VI-!TZie?*Q zS34%kWwgVHv5FIJJGz&}9~!^T`Eiowf1dVbCmwFLbi2rRwX^{vk-{XY*}@E}A^f*& zW6GaYO|ObDpDtnw}nKq zfbJ$C{3gVjA|VI!!&U2>k4zC?SSPD%9QHCEjThb~nud?1Khz`9L;Ht{USo>yo}s)q z0Uv`e1<$oL{H`ePp?xsuSHNx#Wg8yUZ7PRMMq2t#fNb6{8tjZ%{x|lIW%xv7gr)DwX0c==pVXlmr80?M*4{Eh6SK%ev-inw``PKnegHYvu{07%*b@>ABcd)fJgbU`aC)4~6wFHgu25s3p z49 z9|EwdF93!ieW_S3Xw?wODhoHKF6*vH0<3A|N_7P+6TCt(8rysOpBKX{5!;~}`b5wh zvSzkaEBlOd%TSI8crV+-6jjIUXsy*`@hbA=ztNGDG=j95AFe81))B2&JUckNwfWXK8X5}=tFKfZ-FZqWUzgn6{<>`;|A}Z|`{wruu%qF2 z9JW>9zw_wv9j@VNs2wK@k(&;UgZmo{Id&F$oH|itOsUQP)Z)=>JgsBgm6wL~Qk8Yt zT;Cp9WCVhRL$5i{ou0+S(s2$5A*65~>oe}Gq}Lpw)$bvZx5lrzOpCNoNB%UYho?ZV zTHiNg9lf_(IN(<@)w8UYdM8)4JukSx4-Uq6|CkBG%**arq|a@VLi^S9ao-XgTcAMe z`av|>$y73@P!d~RbLJA_1-#ORnC4;Hh^#!$wjnxAVD@`4 zdMd)Xoo~c!X=-(q7y0JBsTK^51VrVM@10-DplY+cmJ9_fYZNie`kwL%W0iNvWPDf= zlekW-5oTh?koX+H$zFF182H=i$Yu^y&z-SFw2=L|nkoJisvi|T)wpPi;IwfdC$sjd z5XbuY;e@GPUq-kxxkHLTjGeZlV1x!~r%5Jf9DrT@{xT6S2&u2C{CTv$?iY<=__V&f zI!+Sk=;izfnk(yh(OrIJIix3VDCB`PRQ$*g{I&^opPqy)qnQ>?z31D*-Z~YxopoPNuQcI%eD(@|1aWB3sAXW81cw>*zNk^JvK2i@Ds3;jljTUS9R z+=>%l&EFt)1p4E#u}&BrZ2`5Vj`YGV9-q0Foj?HC?DpzL)~mDCyd|388K(3dl#p@f znCx5&BQTLoTzSMoI+=m~|4qrju#}cNdo!y2F%e|hFv<;AprFDP{h5JVXC`QNo#+0F76lPpEW&w22HiEcatm?6E;vK3rkw4Ng-WMOPsl$-BowBJJ5^6aOlF9Pl!v>hJdH&O6=R zdSjBtK729VOB(Tu2E>Cx=0Bu-ENq$0*a0_2;&7z^G-$oIsm}4Q8SIKahr9SRI`6op zD9C7$ck=+Vf^x3)EZ)k4n-$f`g0f~|6fMK=nj1HVxoF(vcm<=ttpml74aMv@BBhJ3 ze!`)uYjQp$4%12fri@(`M;GFxAQSJ}{8EBPCOTIEkKVW6?X6jWBMqjuzN4afm%1A) z+UutNxYr{-tN!F#_kvDlW>t*jc6h54yyxKz@C6dx3sPlVfP7g4=18ui#I{)P89S#w z;%+7Sj~)we$Y4l*NfH$@M&kMjgm4gnBLfjqH{B@`a&8Q?n=!VjvA%p!@OVM|9lcmsSvf77&)ibh@^BUa8(oFVT zx}k>l$nSfLSnzP6G(_YAoyv4?{j;T}TOGz;yrV zAyXL4`gt^?Io*RsS&@(5AOxp}N)=kZ+u=kaT6g#12?^Y0lXXqtfD zs=ju~xLT)>JSeWyXfV3n*v3FsgP!;Vd6ANwmDXPIe3nxf7CSs(Qg4~9?5|6^QKP6@ z%6PKyeMfSc<$WMU_T;aPgZ;IhRG=bu2hdx{*q-hFyD>k9d+DS0x9`MM{I9Vwqg7o$ ztmX3-S)7qX`=8oWI~%X-reBDR(Ff5N%uP;-v+k$?T%U}IDKHwSHBTvpp>b}ltI&*- z%jjJ*3ii-Y&-BsnC3823Ed7x|mH`!x$IJbj`?#8g40`qD_q%elOB%B1Q<@q`5oUyX z0LUVmILJ^Gw3}FdAYFrE8p4M;zK2!J3^%R#Tn#B8M>U$`3>s4V0&m~x^00FF2Qh~O1%6O?ce-j+dIpDwH&xxArN-Hy>B0RR@V zZ61!ucQ`+*TzTE4SNFYye?opBfA>NnS{_EBJF>=WdYPL4j7G}lD03yE~9(CDO*r)0f7GIo!{XybcfC&npUQzNA z2d>vDIlChq*tvo?n%vdirc=%HQar-5$~5UoAsCo5YfZ^8O<}AeX9NF3NNJ?Q4)Ny}&h1!Iq(*g==RR;{YSqZtTGl3?U zcl>aYf4TVOqL-UT#)|+vw+cCjE#1xrhnV73^;rvoK|>q+YWob!%RYlH6dxN+9&SLh zsex!EHvye44Mh;cF=bix*%748eckW;oop0XyK8NBqM!j4&u*RiYvZFHeHw6by?n`! zh#ShiS1P9fw2=gQ>We+dEjfh)kw2=w$KbAC+++i}ne1GX6Z5qoU#`K5CqhBNxTKUq z;G>!%7RM4&WFAgx^(Tf0!(e|qq_IVZff z{GMiQc(5DlXcOhJZhmWoZ=A_$4srnZy6V|=PK4$yHk0-Sl1LtF`X%&gxQ-%X?c7~V;Q$m1~#8`+9!5jd#5WJ zJCJm#t1VDmF2}qWXkEOh!o_hZU;Fu72lj6O#hW`ZVww{ybv>!~AJ2YOv0Fl@bd`gA z^1`2w%{aIcgAmn07Gj2}PHCw5=gH|g0clDctqNaYgzOqF&jp;#nXAxfhKhhDIC4@Px01wX@$Z2hP7Y7``>y z+A3>|#XStyjQ%#XN(L*vb0cQU;*XJ7P+u{q=oD(EbH~@rq3Ec2oiz1(MY@#lrj&R@ z+R#FX5B&l;@h+bXXQFmgdJU9H%1}X4opfFrEXMZS`3)N+>bu~H`hlUJhN~k9Tm*|* zRh$t%pG7CT7oYITWFkwbO1jm-yRgH}<*rC4TpVoUT>^$8dtygP^X^V?HH~_>CCgjb4}jjqgyf(tU@RSB@3v)T2bVJlhEqG zy#H8A!I|ig(Y@BXnTFOO3X#nj0ap>L&};W+_rk~|%H<&X%7bwO^RI;XYQbdP;9fm4 zbPH5l(KN?59Q=@FB6dPTuQB#l{jIu{#lvvN4=6pekSD_d_z?r`N>|ya{Pg}}Om{8C zy5+1a3A$W#E3_m#kxErqu5_2Tc$}s2#)iDv=8mK7{EZ>M7mxo58DAX&;^h0R@`4x6 z$^i;I)b%HF7d?Hf^8vvO<8dcdcK5womrMQ4wjv95E75MgDWv25P#;^FZVRa;SMr3O z&nL^<*PXBPjW+2aRv*qzM;yv)W4$j2#5f>j|94KO+o(8F>i|Ev76$H)1>XsO8TMPx z_K;O-I0pS9url!Gr>^DlNAV;~zEx(F`0ukvlv1Du@tO?wQa6**Th;OOg3Wp*YFk?V zm@#Z_-D!V#HXI&+{BLNFTkO@b+Pv6?60m^{;E zO+u`ufMw!F-xTL zxS?r$nth?A_g^+JU6{$ckfqv%3?bD>a;oIIKnF>NFdb%Sd*pt71E?g?{jcbaVQo!Y z0BG_UKfhT2P@7fC=e z2uO5M{iV5}73e9%nmgFwmv6d3A9ZPRGElwUpNZ@rgJpYf%ROZNBh}x3KYFT=3WXOO zzHhyW@tE1E?SZF(vAQe0{^*BaByI4WtcAKA@4V@GMbX~~4x76mGBRyqJmsv8nnzvs zHe>w4)i`*m%X;;#uYh_x9n6kR)(iLZUAYbN126 z(e2$jJz34p3IsrhmvAhrA$RsuuV6A?%QNRd4P0^iX8D_l3 zA%%Six|TE*{fAd^aQ}a{r^v_Y(V|;4{mUTtiF-N=l!C@|60`fyFp>^zAI%AKPHyqv zC4F_D1m$ZQbYEbUn0}YA*O2*YPK)dnWT|9(c^2-#xgJ~TYohDykJ!R~1MKEMQN?Wq z1J9i_W$RD<&OkXZztxN?VbZJuT?O^K1!wVqX`?BY86*t>ST7i!j+ zleCLvY6}OPFi7ay0iRe!Dfgo7FhI_|!(cQyMnEnYt?6@$d=vK8it4yKA1gDT>Jw3^ zwBap^O{vyh9~uJqznA@8E3-gTK@_Y%4fGq1>)(xpa(_P_S_Z+Uy{viEV_u`O0W=aFRV;)1$XP-`%I#VM=!`1sBm~Jk(fp65H!!OA zH!7l<-<1<#)jZ>Jm*%~s47T7TV)0m$&jY{O zLxTFeU-62V)cF84N6%VO>yCe6MJJ$}5vDkHeb3igtdY@Dp=S7kj4Py^!(|QAI*%B6 zlNeaHT%=KpC*?AA8`cPorjQd%!chFERr+3`3{t}?oGspj{gX*r zq2?)TNT^@P5G*xEEx%HY>PzTQ@t1iURSlDS%k=>Z;+2WbpX{ls&_k55$5GzTvc~h% zAvzzlzu2k}TCN4BP?k69aiXwx+Qd9$xsl`XS&ZXyR~RdSD#J(Z!Ln1mt#k$cZ2t7> zaoQVnsPT0+54nPkx;{@{;$td*PA|s*JDiRBt%h7^>&eus#P_aadTE$X(^CBhrM(XlrCb2Y2ik&hkz1ahXU~a8W5gmvd+c;?<1gn9Nm;d+7OjXwx= zefiI8X0G554pZQu`klv8CbDW|sQ34XUlJfzPDDz1an>;l#sSM71&q1eQ?sW!(#QLX zM|0|W8^h0tF8)4C%MpU5NZ0P-GJ-3TUIf~jg;%2UQRWv@=vNKjVlvx2RYfs#L=}nT|$?f)hR@m7dgo}W#ipEx^I;xcx6NxNqc5R7WZ4xA+vsxOgp#6Bexfntq zg+?+&z?o>(5nLLh?-T(Cm*Oiy*eJV&JfDf%NQ0wex$z7`i2m?fQ0TvLYy1(`-iE|8 z;2n`NMb8GgILWDPe47pWa`SI7o_g|7l02P9#-JzyAE4gb!6~3LiNv$C^APRY9XL8S zXgD-E^`@7iT!0K@>_la(Z?q2Eq5-fDx)-BrdI>TW5J43z8Q5(rr87@6aI=~*9&Xxn zTx7&6Y%#wR=FvpEGj19|8-Nc^2HuVUtq6|#UHks!crX{+7;(PT6?~IUkg=UUWn1u+ zkod=Ay@dIrM?ZH0pdB3o?8Fp~jA(Ij%+)V=d3$BcdzSLiMOu98q`HuG4*CI^GpZ2P z3UQtW$Tz&WU?nyA(b{O`{oWGMf$v(zAh%EL#T~~PERL$0y#GK5S_wxb7ABSN5o{y5 z|F1}ap<}YeB4NM^I4C~!3})@f$#(vM-;=iv7bX`<{RKVlTr$V~& zUiOch>*6aih=b!WJ?IqCT!<&=cdih|Vr3;N+7FepJk+zgi3@Rhs=#!-W12N8dci-| zd`7Ku4%&|%tK(2(hRp~H9fIP%C!Zqmum|cHJS(43PvahH_dYZ!t3c~stJJKBK>U+x zW5BR;!k+y^QzuNk__Rj_up*5*Z;7)3kDA#}(5C~@iNWX& zB-EZ|W%fIf=Z`&fI;%IA84dZ<5BAnro()j<{-=&t?}Jx8PIEQ$K7n`PMica7hGnMh zmLLPDHKK~WDu__lX)e&k(?Lb+yUHD|`Cl$kXV_-9$3%y@Tx3`|0033ZOEMYJbQ_(r} zDwyk!(oz8S0t1&e2h&rwVjY*=LQ7rv856{;`Vu?DBqRCLZthm0E9-=9EpCE>Xr+=M zKPTeJ?Bl}@X55?PnFWlBDB02wFrLkM5}5(jo)9W3ICE5ZHD3M8T(Lqwuc;5(dLY8o zl0%dzs`Y=Thn@oOpr`ZQ6Tn02=bzgruW&n3;n!YJm8>$?axVB5_SCoQ;SzqqOt2&r zJJLtR1Ttiofsg(3EinXe{%t(adPO?e{20ugP+O}0n$AH6*p&X!zMG)GF@Z1vR-w`0rDny(}u4O zmSrVfJV`v#jAB+KhV9H^?AoWtRV9zAtucW#Ry{3WD?Q)js@}snsd8>xHeFmy8Gg{o zSB4fQsY*R89?93r1YT+9ODao=R zsmwyh4}$lPg?RW9S3Qc%@(R!ob+esp1vJvF%&UuZv~PJ! z&Iygq(D@7aR%igb&{hW457DJ)?N8{0@v_W)P19W? zeCR;5F~#M*Dp?pmEUz)I+uhAVVM=30H}+Rfi}{DJ6sDw$6zv0l|ooT*d-nGg`kb)SvyG?f_viF zTBafyp7I0p`Q>ZT_Q_20oJOdV&V)^AV31@m^${Wtl3|26DvYqd-`;0L%XU(S-HD2y zArO7$anU-ROQSh!J($95_;zrguJOYwI{I7QD5l%S8ve*&&yXo=8HOe>_ez!Cm{Bm) zjl>}eG{?7F!7Jdoa)6cU2uLEvp*^O=NI{a z$V515G5c@IR%D&obt;5>+>}SCTJyXptoM2Va{OEJUWApJCf~Nsb=I|D=w>j+;-EsS zJ0G~t__xkj+j96e_ILxDPLLl}fmN=oa}lY?;T5VQu;NHI@4s6pdD03dvExBl8sTQY zdqfslQ0qR@f7JdW06?PVCr07G=UJiEa&ca4{b=!+FiwX^~p%n@|Ie zf>#6u(lJdh(7#S+J&%TIp>(&JaieVB&*lz3{MpQHEF9uO+{y3M!-6UR8! zltIgpEs@TbMQ}k?21>cJ7g9av@XJu?gre zH&qA62Yt@L;d)xJ1?(mA!4!NWUYe0OJP3t`7Dr-C`CHp#gcyFkHg9? z^0_?o<(uZSNLefbg(lCj;)0h|zkbS6pX)%oC;;VI?+8E|@o`#5vS4Rq)Z4F-Ol|_s zRlBmgY$p{`VVmR}TbZ|Umb(@Sysv$99Lv2>mF(2 zyJGUKN)JMB@TU_s59M|C7EJqZKn~bBB@9ExF31loZ>Mg)|C;-m~H z3yY?ws~z}1&1%T-!bmNx&2ezL9;Ns=U1TBf`eSEU;-leD>8n%*Q=%H{fkM$czUJr> z3arqD-B3l#n@ts`=-z|#A>to&1DKN&!kX3>Z`#4{5S|RJ>$FP19dL6fZP)&2f-zYi zeGppV--6~?!y2w}$}lClhoSy&a8Md;_>#CNRxj|jxG~%9O3FA6OnWIf_%;!Q4e08x zmVzuzfOo6SoZT7byKTDyDhc6dN%^y=XfL^bHHHN)licjTt&RN*$l_WAG5;VehyI{8 zNh^MV+eAhJZ$=VooC?NtCk+0YzvDt>ZR{`>1&0n&tBVcXNHcwTlN#vP@^MZ^vt+I~ zvyo=Y^3f@_AsDN%s|!Lp*lJhW2P(s%0IgFXsQ@8{@0#F>LX(#hF*z+9bpbhxE{ll~ z#yAIm;z!6Ix;?(3ASig$Yt@jamCRUAT4l?Sgq`oMcXJatD}k9<63u;F@Inxu82*&J zwmbarf-iY)=Gr z){yOTr_87`n1%0$s_l@)JqKG8FsRElEf;TkE-2~JJ#V7G{A`qg?|%~)h0Br3q?ek6 z3Q=BPxa)Gsoqk<`UYo$r-Syp5>ldA-p*t_h6E1`Q4*)Mf(7zgO-!w|veo-b=m6vhh zEP@kwzx8NXO9aau+@1WxXsXYJ5Z`HUx6>hDEUsm}oV1~HK zj&eK@Jbrj7Z30HsJfpQ-O@`HTZ0xI$sabsSk*h3GR<+;XT(`d%Ibo*{vIB5@HClGNn@`T-mm@%USu#GX^YADkb7GuwPOn)dRQmpYBp2G8rTC{D6in>Q z?bjIjIgo8_8*!*>{*)=NT$Ts5?|E$I#=9DdeSM+~s(ef2ETK48vhN`Ahc4V2jQf6m z96?D-gDXbZP73}n|J6tf?AiQ0tkS>!5&pYVe*t^Zf!tk6-&!PwXpDH~)uLq7TQ&p8 ztU|dDKYq%7zoIwz?>Z9bp6_B^k{o(M}1T!-ZTYc!eM~lgynMZmJ?W%Eh4E*>D3+BLPILwST1p)fk?K&ebfu9M{lxyVMIk?6uf%tryJRFR~{M%w}4lbqQqNfFU4=Mz`vffK}S_-b13lA9ca_Y@=e;VOh zD{athrqSyG53}k1GBy$lk<5Vd4$_T8$kHq!G>7YbfqN7z{nbNryhcry zc9p|4r5aECQBnP%rRBMZ0FAcRB8Ct2LA?JCg5`s*g}6O|njCMR0jz{w^qJrFzch*A z1!D>I>Xo2zjRI;@tu4UBMqTopQCVE`x|_jRnQqg%P2bg}{FiOK50yF6}%GNwhV~<741Ek5P$Zmp`mif3gkOS(iXj z1$LfqOXM8$km;OPB(NYuHvCKD@lT+cg;}Ic)=7v;|H6wC!#a!1F4@8|%o=%vL!Jkt z^8wKG_wQ%a+|q5kr#73GBENGKY+G2H$%$nE{IpSdSmNQ^$Q5A_C{5~S0ur`?XXA*M zLozevm{_^oW9wE0u*4N~wXJh&fL8exWzb4883^YE+X$+mA>2tsjW>;wD4{5(oi=6U zEI)DiK!ci~X&vjGU9=URMUDG%uBVm^WK^gTx4K%!%Q;aSL>GASWiD~ycZ(rtdqCtp z1hRanpYRB{(@zvPrDR$Bt1e5&-1S@E+nPEbvCUbZBkLehiN|o-aDoYk1!I<$EYc88 zTX{XY;e|tgW5M?^9`OrNvo=e3*aH|q?<^~T)+;%xPB(&Ba5 z_1tPF*;tbS+x%O94Nw$dkWw|rzqPU|IF1^XV$~6h z`4l4%9Ih~q0njSNR^uSczPQgYH%dKJ^KWzJWmptODdoR=n%d>^3j`xB|BW(JhW_3E zRmRR0GPgv&kGVb8URJzlx4Qg5Xa`4>eN@oq)_9jePv9nQZ0YkFh zv(Wu?G&<&;s4L1&%(wAy4?H|Hz=VbC*!jrzRcMgh(^1RykXWlDxOKi0_L&vkV(fJCaBxFy;zODoS@ zB^?K0@OwCR&8sASw@}0qQ4H0uXkanBWq9x>4s_;ABh_5%z^`IH=o~2n3U$jLc*OA| zA*Zu?=QEVc2nDzvmQhB_s^}vwa1)}$-d1W=U}gqoM&p<`FM|RKuUcLsBWHB|3Zx=N z5`5yL$ZuUS|2=b>XC7 zV}Y3G#J5|j%A&$lHmYmFu7QRg#zq~gxGG-RjtqrK__jCdGgPdX5zv0yc6{$AA%Wps z@X#x#`Sh=Q>lybiSA6|)Gp9t5hb%Fb5P?5miy%Wg=53zrn->N6#q9LW*27ilZ8s%m zqU;If_wqcy9&&Kxpb$BLtXIDGO*9>@)qGbP@SO`wk|N@n2E9uSIK&mZ9`gO zO`5*qZp7&uaM;QM!dA0$JwVb=OUZX30I9UzjM|D^6j0}TsUE$}rQQh>nR%#&604(J zGb4laKQ{)CF!(bzcoxOr2|TNrSK;Br?gi~x9M{sev@R#ny{tXaEsiylPVsL^n$0_n zyoLqpL~dg#2Z0zof%{Gjifh=S=}oC5r15@Z#I1i)i`gQY%DjzIgiayE<}8v2dkw%U zKLdepd)V~}HzMEW5i+gAdj~)8xGC9QG$JD)rc36veNI~6t_iXv>PCjR*wX*8ES^UG z#LB(nV?rf)K(ly$$N9{TcGtYfO%Rkr$)m;|do1$&_P)i%DCL~J>k|Yx2d%%g~Tp?jw`k} zgM~PRZ|4WbyifksYObL2h-j7XW6lql)#)Z#Rg&0!)$P~TLaF8Q%O5Qb)V9+BxTh=~ z8NUyyN(T$AgwJP1A-K)*JkN5?513TaK z&=XthojhK8h7!EV!{Yq4p3c*Amhs#rQXk8oWUs1zKmtAAa&3n4og(q9lq^f%d?99< zoghK4t@ufnoXEyMjhwO_Yk`Zb`CR}A!v-KS!>SQJHU}4LF<%B^1*q8|e0!XwR)$v$ z>zVcx=Qom^GiX}Z%Osgx48L(6dA5(ZVa>hy;u?B0LW_N3pIW*wVAJbW`0YW0Hn)4+rPNGS7{h zf6qL)rfYJOB30sd-#Uw@ggokAy`*l&K#){b;IVisy*PhrR^p$k!9lxMI_!hR6E6kIv7Y&E*J|xQdBkXSc`sMdqkWhp=d~X6E+bbE9`=9U8HY z*=1p}!4Gx@RF1TGuW0wLVX+JPk@W4cIR{SRY};D?1)bHC;$i1WfN>kOXbTdd`_koTs3?!X z9cTj+G6kWq!DrZi+%lU95g1l;I~6JE1$1oPdoSsE~>ZlgbVe*X;* zv8Oh&S`vDCxfl9H4Lk}<=6uTPqqq6&Nz{fd9|G}n>_#4A+Bkz$)y;KzU?t1VxyCU; zCrPTrJKnbq!B>oIxWyJE(vEgtyT4Q#6a{VP$wjy1U z_6Wb+URXT%Un|N?n2SPWn5@;b`d{wi2564YyC+z%$_x%A75s(#Je-TX3&O1vD8K3Q z@R?R3Q7O23*e!-Zz@TzF9p zHtSd21t%`Y?Flv_o9n%)c}!2 zQw!KD=iQE}H0Q&Ku=6|buw5nOYv1e+y<1m*(x)s&lsPt{TsBf4SRglm_Q~=W^$tsq zp?IPJILnSY-4o+q2b$mKa{0d4(~2#I-MzjGO4el){TQ=wupGxmOVB1FU<7_*f|?HD zi+d(?AJPuT5uDxFGZa(uX_CksaxlES2M;b<*Eh`wHZd_HbGH5m>#lCnewF+Vjw70# zIID?e8tZRPVoZR@sqXjK%M0=%h{n$|t#q$bQ zIp@vdS)-*XwaQ%Hhl{hFG=s_Mz^o`iT3XdaB`Rojh8Dg)>MtgW9zj3$Fv-=lYQA%4 zA84t&8*qvsk26HM%-+ACL;X=IJOj|W0aeK)B9eN_!+pV;Vaah$fzv9!xU3to{NMxC zmtBetAO7|ymk2h1+(}DS?sRJ!9sc1OO4e!Cu{1AWQ493uY2|)_wdC%PDEeMN@wl-NqQe9&|m;O&UN7dgZfa(^V6TnRD{Y2X^*R4 zdYM|$?G~|)y-D!k%K1AuI?{~+RIBy}CJWf}G@`*3oxrebnltd|W|R2QjLiOW+Q*e*XJd0(4>1d}3U_It7cdq4ZU)F&0XvYLTK{{qgm#TAG`iBZ!u~*%Dr#=Nf(&rnL%7;g?Td!(l z9gu~v1D|#oL6hcz39V{$ZBop0rQJ`SJV`=wbi7(kU6i(+tOomv&ZTCZtD#xtSaMlV zUJcF<605(G$ukH)AuHR7ZjXVagNVd>?r4+goy3f}oV>M6D-NiU(2jv6R2PRzge$CQ z-$;9*;hbK_lK-dbWJz%bM{rhv5U|2!`sEGqS_YP_>6K9n-3TR-?G#WE{`tRbkTV&D zau<7ScKf%E*sd+0&8t2toUiY-d6(3}nxq@PxC0=msVU8-)>{ZrF5NFOTv^C8t)qoz zKqqD!z1Y;xHJ$F*_w^_>ORaBfXd8bqV8owBu~OEO^_sJ5Z8;ZXKYeB*nSPZ;0$J~~ zw^Px$(Cr>Ep_6G!F9VNVLVuIwR;{+POeK9lgee%+*u_$U&9$)CQqNaO$x=KKnWaXpQEUmmpPj-U3wqUzY3GK&N*?&R(avNk5o0L zD^y)MypsjBip9WsY*Uhs+PMk=?Es#&xNKC&h<|;$`jcfhoz(VM6=Sy{7IP9*m4oTT z7OjvN&pR~S>ko=DW*_5FKFeI_pM!FBw=me|%nuw;S@#>oZo!r2E`I9e+@7Xaim?16 z;R{QJ@;-Vb185$$)mg2;oHEEqtJUB2>%73dK*CIO#ymJ`-l!=l9Ww#5Ec)NDytsP$ zZEHTo6$Eoam{#CtWB)IyqMgWoaoyk;Ul3>lJ+m!B&+tl)!>PTZR;oiIPRFyqNV<)| zxMYFaA+~TD2suD$r)yR%NpH$Sk>!)#CYMao&1}n8)P{;VO9wR77(%c_@6M+FwO6se z(^RpQ(0EC9)F_gso@O|@-YHF? zNTsd%tekxNkH>)@muh|=t^|$49ho!~s2(3D5`<6t@Kwr%j(3!>2YQ$;`gmg7$1@&v zE=b`>yxrL$kA|gw5eetDrW z4|tbB*n?1qDr3hkPrhZjua=DUPnHoJyu40KA$+5x54*;0Ke&B2TeTt_By68lf zLt)NpeM&VcTW|L#T1nBjrMvrh(<91us_5JO^h)_O4_|ce* z{Fl^T7WurUW+nf&)W*8ST)1LCN;aH4oQ?B``QEzZA09sjEA@t+phdlhSAj9=S3HQn z3+)TaQqLw7Z#mGlpf*?st)v?C%n1=No7eB^70Q+Mlhp+sS`_1xf$!y+vi~CS?J}NV zGB_V(38C*lpxP3YpJ27|TSm(@k$3Y{V6Cr%`C8sKPZOoTyjOL+gIXy$l?)_#F{psaf%}rAyLY@&h=Hx zxPOBw_BUgSdu_xcMsBF2+$+s4vurivekcnefd!+~o9ssI>lvtJ!Z~7$EfSuZVY(fg zyaD!^)t%pi)vz4G_A<+sQRuymf|@TUaF%KZfWgQr$(#%kd&rG50EtK=Oh`jc8QN^0 zIK)?mRN`l5`U!@cyZc*=!e>{60gc(qMSY!K&eQrNXS;!}8$VxvDG%5-Ce=!^*vQbj zpaFKF_l`YbeDLSLPr8u^o%ZNc5j}Rc5*RcI{0wa(IFh7R`=-KuNzx^@W8bHicVJ4{+{u&bD`)J=1MD#$PKm5x)B06Z zgkZEiIq6Jw0CW`bs_27>vafjE4r))i41&~tlIi-?hEf_362czx z6pKiXpp4~u9nP_o5YAienk%DWiH;y5nhY{OeS7+$IZ8xP@M+tBS6?AMPso{qvILm{ zV$(Y@!?7>P!8h8S^7{{DgwP4GGNi}EKVbk&<;^6nKR0j*B~$%taC6fA2__tZB`4Np zQ%M|b%Y^S$CY(90rWeS-c@Fr(y8QI4TdO`^GJWp%ESe#HuM=2z+4O++Hri22CKl*f z^WDZ0?<8Ym6;-=f2;w@nA6o7GR-%pdi@iDxls+|_k%Ed3k|Ee6`c4&N{vi3?GHn3Y z+-6dBgzjxktAM3BFDpv4AJQi1ide8dGGGn6D3*QeJ!U^9l<$oKI4tD? ze3{^}#SFo~#P<}`5h9{5Uk-Jy$5N-LF!=!lR00OA^Bb^5Zl?@?W2YzqKw`ol|L=jz zUZZ5~-PP3?I@glA86*RNej%z5v@KKkUJA;>^DLglRkDIQKd+h&ObcO$QvSd3Cph&- zzgWb?%)&%~#!mY{5nmT*ALAQptPlYQ`}35j1%pjqmd2Ey{$-Sf z?qz(0uT29&TJxKLW1%FCAaxgHQTSOyMPpGy%#K7IrCzYz2FMC3Eya|3%rTV?Y*Rx= z;qZjW>q(gqq1s|`9JA!`(c6V{XCrPu8A=J%ObRcTktU_CtwX<>HohmJX)e3t1N?W}sx0T)ASu*CWDf5t%kcLqS$>Z7bCY08#lxcU>_6*Q$uf|F=_>zwu zW4Q&6I7VK+YJZwuM|b+xM+%$9bV`(lsZx=bUYXEScoktt^1v!5GhPW_j z!R6H|=Yb`%gt~~IbrSWN3jH|-=a^iXW+A65&1eq&=A>ne!sB9F*@tqVL%O4RdkX~0 zm!I9VSEqGAX+_s#VP`e}aGbWoqtRD1?uBSLugbCSiZJTmc}Q0;_C*jLi2r2T-1zpq z!T=H5q&ohGdotEQqPZ2jgN$N{q zs+n0^$bQQDJq?V$&(VzWk|7ZXKBan$9>4$q9`o-iR7U_)tXhSTD&0nH#|Bxu3ERg* z{f}MEPmHT}c1@|&) zvbZ*RQjO>?@U8-{zRR~_fGcEKrdi_;@@$cu4!rGVB%tadnJ@#!`J+NMYn*SESzq5; z|M09WATTtajSEvzOO%91P%Kgf0ZAPzBglRuLI>PddqUySxwc}aJ0%G(OKtMya1w52 z*r~|-`OFqYgiM?BGCBwed{uqDg$LKe53}W=y50ry`8Pgx9r&v|qGGa|j|}B=eP1v) zQ}4667zvypi#-|@?x03BZRtKv8)Qf) zKQUX_38V*K81UAUbHTH*jE`=nX3q=4cG<{t=i!K}WVM7cZD{=Rcx@uJQ_Mzy170hw zN?a*tSGc`}tc0z+71y|G?FWA7=+{Oygz<2dh#9T`EG@+=L^DC!LR+W?Z04(|1MTr z<|yTL-jrz1nmaUYP=xCG4Uutvl=10Wlu~F70iYzzyC$@oBFo41Vwr@KJKHL7bFhXhp^h#APP(Q`pOC*-$tMg8>bGWZ+oMlI3i%q@ zv4*VVJ{PCi)+BM7wFK(?DayYgO#!h6vRk`L*@3nXl?#Qh6@j|?0a3a5E5SLf$b-yQ zuew~B;dHt{OMqkZA{35URF*C`QQZa+-Nc_^D;M{PQf{91iR$mRK)zti?Qob3`$0bA z>H*QdF8jV;@;w5|QC_*Db23>)q>kbAd?L$5aBAm~+A(qY-*$f?>qKu(b&3C?XtWVgpPffDo=BHMU?a^`7D(T}1cZ%nNOmNgYKPb-fb6H#kDEs~e5Ku1^#tts3 z5lS34J%Une3QSgylQ7X<)>-hKt51czOwj*Dn<$EH)Brnw0EX=-@;Bzt6Q8K1F1_HC zpr4w6(R3GR(oS%XW7v;v-9q<9|3{-u%Yc9-T;1D&V`|LrfgQ5)~zA9WWMbn zTOcwk;mfyd!X3pml7;j$hpGQMF1DT%nTh`3;jw&X0tVF>l9Y8387>J1(9 z)G5=JBpWG@y^Uj=63t_zHx+=5q6W=&v}Z`EhNF0y&&j7k8fOsNti$t)&c~lRv7lOu z`IBVi0X$%JDlksCTlvx(Y0Q5o4f~Y8>=!9)_OSMtnOZE}Qqk9+ zw^Q(TmWun<%#4?)8PC}qVL27Rr;no_@E=`om5VO-KE5CQi=ODSQ~{POrq^iB%R%mU z2q6g(e&9Az7RKEGCoT(;0z}$1uw+TGgM8^AI??6r%ZNZ(Eb!GH9HOAVxnlsiKou2j zgDhm%R-eIZj$4vnMxhfaPZ!wW$)6_QbaB5^w8V4Yn#Y1hR~XZ&#i+&$5scz=sQ54@ zGw!n)UVDK()*Tl0tx1CG;w$?9_eWK;)dfP|qBa8WDyl;0Bm70(yI}Q)R-{l5f{PS( zj^b*5#_uV2Tt8gK6-!rUgySOOWZ^38Q|8~zD^Ob@^@&(q53PORpcCQ>Ix_Et1UZkv zJ2PJRc8Dmh_x;aef2Lnb4>q{ArLE5C#^JXfli$wKKnWL_-e@jgV45)hHb^J>A;al2 z1*Y$wdbCyhsot4tS5@n^1}6GAUKZrIS8Bs!wq+0acS^oRG~_mvu@w4jM!Y&L%%p^L zug1jxN$ewl9Oi1Hp951-oT{4Nm4nay*r3u|pGR67XbR2hWo^y~Pz81HF|NA#0K36J zgzJ?@{==2F0$66Xu4F(_p6o0u&iHEb;0k7%%OQ{p^UB)A*;#Gbo?!M+{_|B+UKOP5_8AI!q12Tj)F`3z9QYOlZsaIcWf(p$`$Dy;oWbXhI%4Qi#$ z?4_F6V-}PChE$eK4MkJA3;Ta;*N-E|p+WgfYw(J(q=$II&Q$oRkth&ATV!l)@VUWk z5%hCFkG_k}31v!S?(>=Epti85k|H8Dh?1B$-5S1jWh_@2m1tv9Cw=2!$L5Vv*zsnL z-qeK9^MI80XAO0gxVgzkP?H^!SE_a}H*lx18E= znirc0|Edf`;hPGX9ted(AaK~-a(_K7SMk5`Mt&Kc!C_6bA$T(-kR>oFQy zmWi~GMpN5O>|V~GT272VxH8jsVOM`AFUps6aj?k8Je+pXhe~_uV~Xm62n8Yw2+$SwAKY?kF9Qoj6SE zyQI@D3ct1v92SkWn5dkybNSa>u(SgKyVJ8Qpf$Z<)*&#DOPvTpo;(4c4R*&uyzNF* z_s)GTEsAz0^e@~kuL3XO3y+%)TEj&AV&`n3tAa8$}eKkz`UdO$Zda>T-&A^oyU^F6BBPfogu z=YIjF9M>lJ@3W-6+Q>aau5{$nhJ6*C8#sM7Q2Ni<31d_=UWOPm_Bo4p=CGo0%JDJ= z2y>RJ)F^=IQS1t`9TZr4UykKKlAfD$!DaMpht3SxIduQwhcMo(H&$O-U+J7}Y;j60 zKAUz(OWlpcp(F#GRE=ai`r2*M!K3;9RHau8yqlANs*3(OLepy*<1O_Og>I5lKzDv1 zt(~9;oMUB2{IN z36|erT6B)No6cmQ6sS6JdM7hIdP3>ltfM(gNE9rP3<2mPOi8Bi-?1%cC0` zpiY2K5czA?CosD*9_l56EPO9i;cH;aE}?)Mf;hC6hheKKK`Il)hxi0ylU zg&wc}r&?691{;dq9p3~_wsu6Hv zj|1bu4GR)U=!$)9Z?XKm)=#keh`c7R~+T>QlizlajV#!R0Q=|_58$+YM;qTkj z*X6UOJC5kvl11Hy&hA!!H|p(t0h_ZFbw`oMz8$n2b?j&xT6H@1t2y*2F9`}xe5s&6 zGHTbfu|+g}UXFzY{0kW7Pm9HxGNHc5&K>}|pOZ=5TMLUk%L~z{ z4(^#tfvaEax}4MXtBTCn(AQ_){>un040~YKXIZOPEX zoBS!rwpDlHZJJ`C?b@r-sP@Qk2$B}=l?jY6cc&s1k^Z}*?~jY&ntzVfM#2WQQh?=B zw@a~aK1ivebHo9^5Nou=XwHkac-k6H3oeUi?BL{Ik13PyBU+^MZ*uiK!z4ZTZr$$i zWO-omekn(R^Q^(YdyYW7)5nafi`>q1p{?JOQlE*ASP$`s*{HG_%w8T22a4V_&7^4^ z1O}6XQzr>&6!)mQHfm9dWXCX}3z<&-%f*PJ1DN?vitSXB?{}&@^>;$NRw7l^2C8RX z5sB}s?GOYn;bYkvAL30+E3TQ?JjCNq_3!SU01vnaMZJNsn~%?&q}RWRku8xZOHocN z5Sg;QYeCb-tZG3qJ5F0x~g&s2lDHqAsZrh1|!EK%!g&IZ5*ik1yu+i7%X zB=p|96z2Eh__9@MIs*Vi;#m|FJ(;t}F#Sz~y4pg7+1+Oi6PKQio&A?d&FmMokcnT? zzkzSVkg_%?rw)Y)qP!Szf&dtXdfKnl93f>2OeGsgl47CLR{)}(GHD~LeQ$_XV*o6#O5R?>6?nz`~IqV2< z3yf0&iEZHK5pn5Bl51s({rY`1M-!+U3E2iH;cDJ&O1!E53V=|of%74EdkdmL6@Dmq zcBT;GWcunOvt_kg3T*pFFC;U2oCKP!(WA)JLN-8(LxWWTa~EyoEXCp;Vu*w*H44o` z3iaKNd2EESxqKk{XOMEVjM-*2K$L0YKT-G_dH2SmZ`V0`Do_skw18ZYPy1FWDKRw% zs3cP{tf{QaVD;SiEk|@}K^@@s+J2U~(R_;R6&PWL7r!Y6J0&(+Q+Y05`RgE%HP!ZU z)qscYHg|K#C6FG60Ten$AcX6N-(Vho+Zc6F4Y9sCg{!971@~xYmU9A>vrj z#TU)YNC~#~(;XBlOqvm$==B&nCO!4N10MCn4~Oqpnz71xG}l=eif4}v5^WcnHnV2Q zE88%1Z!8Ho+`-NG$p`>{9@=x)M~*C!T9nhFz{6;qF@c*wg`_gLh5$r(iBd zlK0()zD+G8sop-$yA%-SXUXJb5wfMGo+_VJNHklWwy`R5#Mn_)Nf$uj{ zN}g&n^MP2cOkVE+B!+Mmb*9U)*+_}3qH#7lf)jmCUh-o~&mC7;@(}NC2s;b~spB%s zq=+QiN_T(VX@?IhGUJkFqgqLiQoHkPCY+{_ZoL9Oa`69-x8g|Tuvb6sQ&oV4(ev{- zvE_AvdtiEv#?~f9S`Hu90(-C*258Wp6zLqL?Bku|^;17*or6;3^g-4jtV>b!)m`3e z)6uKxBeqYHc!tk&uDlsHLR-jz7X-vB1gt3|HIpsd4M9m$YkyH(m@lMi2~#3aT=B(1 zGOod*d4yN-hyoKm2@?6pmrTj}^z8g1lhc)`RtCSF;OQu9mWTzG6Nl!sK@Wu2t>4_p zl^&oDl6d_5%{)j$xDsf$a)@zr;O=Mr3mt+HsIXSWWfHmY<-pd89638H|6Gj;d2ZlwVnM+E~)CYE|@PyO2+$;q5q2bZiKq>-asDe zB-WyCNg{`PCa3*Z22JH^W2|InbWb~cn7|Up+2^xAF<_Zr-6t;p@@ViDP0YPCzXGW_ zPSoRzP`N799X?g4{E|#)NDa$m5$Me_o*y^QY(@wNxlVnakz~5He|kx=&(XxQ>k7YerCN(XhE9qJas zb^Uvf1ipWXbIN6nQHLs0S9i#)cLkO=_}AO7gC4s8vaX05Nw?+N(Zk%r7(vfLUM_8_9=foG#=W%gE z`m<@?5jZtS@Y9j@>CAI|Dz#9IfxItg>dQ{mQz&v4BMXc_IX&ey%$Y#;r-gGpXQhUrvAF&3t~^^~6Bt7{W-mxwyvQmdWU?>`+g zQ4R+_g}Uw-AUnRf46ZqFB3H$TPRZ55y?|n(-`d4@jU8P{ z5+x$uPy8p=1tD%e;)s3BLj9DrI|tiOITY5FfNsTH0HAYLPN1Tdc7#|^q5f*4NL#Wn zz9qrLb4?Wi3r}=6audO+?y8;O1!clYW|ENQ1JJoWMSj`8?sUu&Gj+Vb%O?TgDTi%I zZMlJsgownYz1R6}A*fVv;gEidDSIs{ZOneb>2wU5RX#+|K#@~(o5Qn+Pl=?0wb|L( zjeiKqW28$JHjmKs*^7!?I{%|2L$yq`82tYg)T}TKwWSNoTRrvZ`at?wBXO$h3#9eW zW27V5=3izLr{B1N{{NT>#^&U{xCE5*A#^)NIC4htx5FD%U&- z{X^7#nJTsP=MjbJr(%j3gd9Le=6+jYv|JzYCwIzOPElPED9=`;^`8r?C()WYaay}s zjdNN*@(*b-BzBg*5ck#&gN1dCN0Y8MlY$#wIm6)_^D*eg_z)xR1j1wp--tuu766ij zC|Q^c(Zf9YCvF4Ly{wOUmpkeEJeX-$Mx4b=D~C1bnDCk2R&(VY$ZJEd?-}z~$u#j* zoTmpi%0qEeWorU5+bZp11wjwLBa%4KC-N043G)^KdXYe;HFs$HLjGt~E~GvAk#C?fiC z*je}wF1#&pYZ@bP`p=4zxrj5|pUhGr7gE})%#3ZD%sF%p{y~4!75xq=Mca(qxSP#e zd#tm4XU2`R@}4Ag$p9v&{4pE*q-FfS5O04q#t{r?RI``rfo-79HG)ITDJIh|maT|1;}UX~d4>HX=rkEtj2tet3Xon%gZ+@?xoSi|YZ}2> zTYJ${f@>-=D59#{+qRG#2A%;=IYzf*(yngG7Du#@h2oYTZnQ%QKy~r68pvbOEt?fp z%$XlGRhqB!T)===&gBKYGAJwx16nbw$Ow8(v|4UtxqVEiPUQ@h(=0E*j9=G3;=~2e zT44PnRMy~+du3sPv$KrOSUM-9*#M9V5Pr3HnFQX3hSeyuuWGLDa5nz|8|vf;;X{Ef zMf)lCug6VI$I;2Ka|zk22S9XtoSf@6l;5A9B~o^qA^9r!vHykOkMcXbT%vu&+HOjj zSFm6zhhZa#$kC9ltYX)BTm;QMxR5N9P@E9c?4XzlRyks0z_1Bgv%Dl^@q1#^lazTt z_KoM*Zmm+1UOV%NH)G2`Ko7LYSEuopDBp=zO*q9j_wt-Zx=EoV7+{bILyJmwNW1Zx z8krug7}E33xmYGM&xn13X`cina}jH*_ud+!6T0ThVJ#PR)hVu4+gLd4G}Zeco<@02 zmEUfBM`)f$wP}$PP}~oTPVmbD&~0uqw9Kc8U*h{yN+i5-C`D#zgf*$utJ7LQW$E+d zQRYb!V{|d#wa>vT&~gjThm{s(yi++~EAQ{io?T3;mn2lTb{XD|733DC*%#9R7*8;$YBzs!bUBRX$v_aQHUi3p#DZfx2afK2pLgBz`)bVu|JfJOD~SwZCELUacF~V$udn z)t0vLKhv<3F71rM)oR9<4H1Dg4|BA@w74l%hz`;hXtocl6U5Z4(#)R1DEO_6Rqh-( zW5FxM39ElCVY2dn_LYqEoLxpG5^X%T5GC)JIFcLB1o(!+!_#S;=&(%cd&lW}lkYg(DqowuMzmqW|MeSjms zVy4o^ws9r0m`^m&@H8vBcMC*?5yz9c!^i~4(PxwVI4S~nS}ILqE*d`0MSJeib%OL( zKCOa17U{DQcV7gqZ?!KjG853}?cHx%TWi~l2?U8-p!DEYKXP3Vc&A} zQ(#2Vr+eG8A1}1oIB|7L7ZMLW1F`*cgpwADixf#l%E}Fpw<7z&UYTsl8E>9X%6OwF z{tB0~{b%J{iuJQ9A_OXPRm&C67LT^UD^R__;BXy`h$m&XI`Q3IeTIehr>;cc<`99( zog~o@Wl;q}#hcL3Wkuxvd6$nA*;-%m3h{}&X$&+Zl#tK2d)7d?%n2I*(L{E&6L0Xu zBDc3)l12+_52@&gc(h)4_)v!#{Cp8Bg2Hi)Fk-h+m&X#|3R%ifO(yL)4uc%sRtWp> zhDc|pTgD*UdqoKM4C;xTbWj5L zHzozaxJJ_K9}>k$iIb?yP7yMG#=z0FfklSSPr%Ox@Aec$xYg{i_|fy1y6@`Kf4C$v zFkPzhFR} zlOB|5k>56WU1NYgl~p-?h;F2Yk^4=9DHPYJt8j@*uHj)e1(g<%M_VN=^n)H(`xlkg zPlz7^>p;h`A$MoY&jX(u+F6$;r_Xx^O4TXM8mzvSvGZwF(4^FQKrW?xs^}hgoG9G6 zfF3r^k=R)7C0FxPKUIjDMAwwk^4w)?qhOf7Oqq5C z5GLh)D^-|B2IT|QgCn@JA#eKzYv$MVwuJgfs`s%ba>irrJnx?B_ZLhw@5X^Fu7c#< zPy?q-PfYZ^$eK0L-1hVqe>H(8&Sc|FBDo+fOOACf@tB=st18rYjmfGt0B&_zi%Ddn z^qw04Du7LWz-J%?qB3{1#S3}+i2+Zby5=BN>x6ewe-TfF}Hz8SV?bBuHN==XHfUfcKtC`iZlRQd`ej3@!QuiO>HV(E-!J5(Z_9PCh%*`*f`!L|JyRH z{gawxVNp=amse^+C%9aICN#58&*4nTnB@t1vO74ZLe$>eS8#N}0Si~crX$IFH8PZ; z4=}gY3$<`~F+%Ecyj?(&6C&0L(XV1TSN+x8Dmc-hH!lr=qybwfJ7oaAASTtbPc>2A z20@rQ`ZNYFby>A7=(W)#jE56r|Cw!WxGit4xrZE4DZvkU81Z3VE#oG6J!+L+3->vD zm?A)|rQ?gB`9{M>3B*12fTPvFw(wc6}Kd-m%f}yio0vDtL$Q292=MFyhcYiy}GKi`jz>|KKxtq)R4v z6dBl&e8eS9@7FKMPjXEK7=iS;LL{sE9~6Urw|PG)azomMjgrOAl-r1-P?e{E-xK5XT>!Y0$(q+!C5cvG7T@uCX`3`+xzR{Wh>FmNU zqt#&pnx*jtjo=vaJb??&o0UBz;)goq5;9-R7^mG|oU0cJ0j`n@Wvpcv zF^<^>1VmTst`1YLsvo|@s3+3)VSYHlBt}HFHXozq< z`O5w%0>#A6{bk2Oh3lpmaSNP4a=U4uzL7h7hfs#3>%>(qQ~#9yj)X5^PN-dc~`o2*o^9Xetnja?rD={pm(WN}Lzk#Tf*xe@pntnD#_^-&c<7^_;O6cAEBs~4@tZgm@)aXxqaK(}is2gv&g=ZIqVP+xsqLd{y-0Y$@8 za=X8{Hew7W^SsTjYU7p?8I|$6H(jH)rv{N8H@%=q#^SXpMxt3psPfqDJ35gex9F7P z#cRff;_ycgNqHR;;#2w6B2dVhG`;T#llFlfXNW&q@)p%WvbvEB;CKp?vN>@qC zx_IpV5h|`KaF6LjaNVLvk*Bb*YCisgCh~lu<1D+z*+vGHg+4_$o1;c?&-T8&GfaZYx6y5Rbh2}zK=(QItw&SRJ zmVzz6*vp>k>h&M$pbFZL!<6~BkRrtZ&QAte#g6;b;U9g^51O=JBvBmEVi`Oc+NY6+ z9RceHHmN#Sa_gs&mhMtfcU48myRRStcj4@H+&9jh21wjTM zQYsLowx4pm8~c9PHei>@oLQ3f*w7P%y1L#799c+~Je~W$BpmJk5*9w9DWI_PmPHk| z({)Q>`n0WAv`#0NDGvW2v6pSLO9YxzY2d2^7P6$fq}n9JtE#c-3L*$xmD1QDZVF!f zdjZd_e0Bo4(T8Gt2w>t$Z0%E`Qe3F$-BMCFX!A?~lc+K}Edk>mTYa9WEfIPef!E6o z$U=E+=7H$PxmWC+vj$pecd}zb=IwUMlcX|JecHEI2P;622dec^qySAEZQAHGyHj*0;m{!)7$I@Zj9bQi(Uqs z1+11kIRX*NfA2UMYz6Q)421tM=+&ZG5Oe893=j)j1QMd%G&n|8+%oJLV1hLoEwO}> z$1dsD27)bmF}1o+pmrCtDY3a?JLfKWB}pl>1{IgW_c@a9j@td1;x^Ob*)V)=*zh#m zfh#JJjxF46Ul1|s)oXxtu(T6$^`sm9(*03_FIC%aso06Bt_zLt!%`ZK?uh*2s!Oxap%y08$3JfD!@1r6jkiu(8;pP zuvwQ~t;`cmX_`%^$lw|h=CuC7+>Sa8hEtHrwbvV)wVVLoUjwDu;qNOKPuO1QN)b4P zde(Fov>U#FeS#yD_i+JwkX|_|=plV4D0zTeEa(yyJGD9Sr2)-fd|MoeoAz#M9kWwN z)T!WEjOHcn35h*yD#)@Oe_9|qz@d@zyMVZOo&r895x%%4>83qJ3uE7Nw*NL>U-7;t zem6x&pD4=wg3}WyYVO+*i$2tySk=c@)sc(v^y~_gZPgOP$-bWR|8|N5un#yc1*YHC zYj-AFs~w7dEvW8-bZDY+qhrCKkE#0QI8D-2NKVg1a%XzJxX2L3OD?8?Qg@SI#oEVL znQ8W*qTW?MHE41-vbd9Z4UIU7-PPdA&vA&-_AKG!WyZT~%nx6Aroa6OFV5Ub01mDh zW3pllG4)To+pqL>ql(HFKKb?wCKO$XG+bb&EHlOiD{t*_bZe#(4+kRkVX!R` zikE1MJp)rhUE%CnPQ1W(mDgbtm2sdg%&BOU=^loj2PG~H1#Xm@FmuEq68(4dsVTI9 zwdi4WY><^znjaQY_XO(FI|A_bTv&JL1%P(apyZ{#J5bHqoJxz_O$(G}C}rtFZUe!c zbpRY9E`ev?rgj<>1QLOdXDK;B3XEkJG0GO-acpWO0<49Ff- zPcGBQpGGgB+8$&UBndI^7}&PkmXX7pcPf0VRv{xnANYT8T)yPxjOl{~)m@|%g6xCR z4biLs000000049DRt@&me^vk(av-`Fx|&I_Kx&XaK9I1C@$cZ1M*$L$mQr*geJ}{G z!ylgux>eRGDbrQ+Cb`jBO00DcfIFrdDM?DQpd2Mn+lHVF>4C-hGIP3+ zMUU8Jybb+$RQCwX{VOrc1_O5Xs3)-7AmKDD=i*jcm1xYCG=w*n#NT7H(T3c94jK+8 z#erV${E}rWAgb8rGH|rz&>;h6&lzQ*s2N=O-;}BH1N!;uMrNg`{;_hFS3C_T#`TB+ z5?d_${csv`LHut~&yN5MVoWH+`2$-Y%DGIv94U`(&7TVK)>M11o)rICf1~7vmsR+b zJshVH4wG^lzs3UF>vhOR${GeEHk%TV=Tv30j9!Je^{pQ#yWi9^lK|S!?sd6fcW5lP0y;yFI__DeWs`tiVmFW4gN{?ncN*K0j?w7_q{%*G}_|$M-$b2a} z3+crixNjqKU3zpxf=Dwh=^RdTPHemRzwU3!5g#>Jyx`Oy8Rhmw&i)qj>?FQ#Z%bj1 z)DwIW=B~SvFeJ|4l{g+w#OWY8m({2RHle7Gm|3qVeJa+c#=DO88%l+9y)tK4NHAIU zw(a_8j(kXri>13k;i~$_R9zu-0Uq%XPkXxQL=A7O(^a~Nh9EtmnW1E23Q!tiemprE z3ZIco;HYVFzc&6<>rWuH^qP&gq*=wuw%_BR|80L}vMo~?K=8=ZBWsQ3Kb0jkIcw-;e)96|qvc!I8EBj!ZVBGb+m+?$)Z? z%eVX`Nf!Y2Z@3Bzp#e|sWHdV~A9r2b^suzU8u6kkl&QdmO|h!c>$yJ$K)V@gBQ$?m zEW_d^HnK09taiHltLL>}p5(6vS0s()$h(L!YqMjN8pko2-(m8cwl@>=AYhrG$%%I}qq@qIAEA zsFrY=Ei^@`QvM3Kdzr=Khoj0V6H5i^CLV`*n9AKnq?O06sR%D!kI`|NaZXK)!n#C7PO5Gp5^0 zl{~6-y-M`|kJ`>UAn!q3>f%6C?oWdva`N|(3%I9mL5RLi#Z$4xKe7vk28g^t_(9&@ zf28f>@_~rrEXpv&d3hPI>g|X?>YcO5N9kJF76&mns|G{rqJ+3f`J2MJ8MX`ckv@7i zRZ1YPC3kuMPeE72e(Du9P;BnVG^vMAdd%(8TPG=#snOxAJxk5}qNLU83AZx8 zsLPoX%HMzcDRl;A@mXx)`hDxuW!x#~rV_gLf}-78Cn>V}CS2TPrUaV_5fn(|6rZl| zC1PtXwRz##upesknzm;$O+@-sVASgIHEZ5}=8?pS{xCd@ZB__K2?^}TfqY5+y za6=X(WFgZKu6_@<^I{K>zoaFWYGj}VNQB@UnVUSbI71Np3SSPcImf^F z8QYJhgM%kbJ29}{oL{vB{esAHn}Jac_{D7IU6L~4kZlS>jpH$jn&OTrRYhkRkc8&X zHbyViKlZC0f<`(7ra8%$%P-mkjOZvhJ-Bf(p}UO4q2W=W9iD8z!<=<2zv=cnBZaVb zEd4A;SD9Is4lfxn&FDrm;0>Y}Z%~qi!@&9wMqgDK$t=9AT}jI(aor9k~zTcRj% z5>`N-B0@bqfk2)BFgHkpR7r)1~LFiBNZ9|%%Kq=H490N9kl-GQAuoaKcq1WUYKK~wYd zM2|GE>XU+pC$fw+f-A1FumeqAnto#Y_C3N%%b7g&`;`g2AGY1gFG4x)*+r=uAH0wI ze3qU^)P${0WV3&+F;}RWXV$70x$vm^ZL<(RJ}P>PwZ6I4q?N=#{4XNI+^T%toZ+qK%k@lGY@MPhZpQG-Z`C{UWVA^l~a2t}(?&Wdoh{pnu zERTpjc*q(_*k6W@3+C@NFSYdt;UYz2^ox1kB|GIvCE8_;gavVxqLgI8Z&*%+D6T=B znPh(U02l}ZbJP0Rqj__(l{}kS6~7WC)cNck4}Ax(3vke#^3+D()e<@}_}DqaKvX4k z#6F>(A}=5~=>NSa;}&7PUi9J8(?nWCxh3yc|GkZz zRnnbChh^T^HI^{b?w49k`!-@6mj3Av7d>z*+A5d1xTN>p{UQustP}*O8}3^DbHgd+ zbx$DxU{RZ5!o1~m*Jh=~hJaB*!3=v_+UKQS8T03tcp^>4JsgAYiLw|g$wK)YJo(kQjLS; z)Fq0;r$&kDxN?~ebYEj(=biS6nT|KWj`RzV9F}$KQ`3`W;`X_pHi5c^3?>%5arsv~ zB%`_IGcF3llm?{TE>JxlIu$1#SP3?L}(kXJm;zp}&>v(8wleb6^; z*EGjPdHUyx&Jif|dt6z=f$e$8VD;?TqK9&0$3xpdh(a?RRYw#>F`0d z&L0+$Vidw1Q^Ve{?o1uDW2~&mywsbWr1hohC)tN^;L8Va|C#;db0B;)_!9kSH=C<_ z^+n7ES6_4!*xt3xNxByHyMX=FRZjNGWeMHjSS8r6b&P1A%=6ce9%w;LFhE15Orb~+ zJx#zH^jLpV$E(FVQdDaFcJsP=jgmbcMZnZzs6RyqOr6Qou8d)U{L!6zh~-bSe(7>7 zSXllVceirh2bL~)Gstqof{E@2ujO=PVU=ba@xj&6#UGnbk-QL49G|$bS%WbT-1-Np zL%oEL|F!7c?B&3QFNNpV9gmWuLEl4Ff;u(0t8gjB*g@)S{y4jo@g%@)x7qR~3o`M2 z$=2|B*g17C^LPucohffKI{ck!%{D99#Ux0^P&Sy{GP>Q@2*qp)>#dN8B@GcQ1j50& z7x#dk{ihK`g|Y6L%tB-5S>0%&K9YoUu{gcPdyOy=*6}!~e$6lYU1ZJvb)|gNJU*-u zc&I?w28hbry@Atg5W+5*tCevoLBNM`yWfTG_KcpY=%XGR7BLg2q5_Cy3Y8kN8x=J( zswvGMVonVPeF2z-S-G~>_6XP0MNR)*mKQ*;0Y;!)s8RV7AL`DZ6}gl2;e`6VDSjmb z(*)8PkqGj*Vdo*!`T$bD@n^-^dA05Y0r2;0fj_Y|x{dz)BXXur-e+ zmAiX-!I6c;ZzB)Cr&I-|Bk7q&Obj-s2hZu*;me`!-8ilKnP;n=v_TimjGg11jrMRkW$jzCWk!Q~4)T>H zI;PqvT>G&0t8_NMvT>`fjo_%>!CCwW7~Xeyf5%@d8J zbCrKKJ+}y<3x*w%Oc6^JTw8%Xd-8PwM2x&}QTn*L(>gjGjWABXFKIIe1g5B!4suQA z;}h*K!-`!|Df8AyUr`R03TqTrKMIA3249A^BBkA&o8HLw8dc{?C{Io>+n{p>!?D=uH@e)xh(rr}34A)6|gurbnb?OOvY8OiRM0Q@_PPbxdDY}fIT*H_cF zT+54^A0><5@`_^40AJN&rYpa&jiS*o?pQ}BR4U1=qdcGWsH+ux!w|s^kkm4C`%Aq_ zPuxMgaUAf44?+577*)kU$KNx!XU7%ZJM-q*upHK5ys-gp85G+LOcoTvix+Nxw8H8_ zO@?lF^hKx$>+x^pY;<@zY#5dFx-4C!S9)_Y%G+~}h7b^3CUgf$9flfo4|ghZ3M!~4 zk!O=&9t)V6Kv^r{?hCEIp$l8vlKtI@F{jaaU_m1Ua_2Tx%Y;?aT1lY2ceHcP+z*ogjQei4(D(el8)9VC z(%S0j;yu#~a@&OgY>1D9#Gj+(kA--1Uh$&xWG6w|&Gb9lIFcWO&BowD1n4%c$wup& zjc>*N(IymF^Z*VCEMX^oHbRY-XnVf(>}1! zgl>)$JDVVPyq3R!xV3u2pE4j8u!#LLqa#i>h#22H+e$V9jp`$|4HMzzsPyZCj9fxrdLOIjXS#R#Mg zCi8RBO>kz3e;#06he#vSc-hnWsDVaspigT>)KL1GU^`FS+~tQ}wgpfcrs}Jd3A)I` zEz<|e3e`_qO}UCHIY8d|`Th4?MpEEAf*yJ1?>}PayGc$d#q0#ZUW)HUc z&b!AGro*15Bn8@Q58|WCuQ#YC04-jqW13u`YQyRP=&yCiDbF3A0kZ)$B+&pQvdxRq zdrR-Z41hk0RE+=b|Nq``90w*?;?eDCLPb|ZyV=~auID$v^Cu`>Ga{n|77p38eYEcg zA4!*}$GtV87(%6P@$+nm=m}Q%j?B9X_<@q_IS0vVMx zQhCYH3_;c=#9=63Qw@#ySSq8MxVcudhH{dRxeMU(xYf5lX8ZPXW>>@_NIExHZ>iTA zrv0UDzH8#*^WSbHlUXuVR74n%(BW5^I}&`QD&_?z#!lm1#JgB4xQ-T=Xu{IsSN_T% zB1L;Y4fh-)crKy@WON_Fg;NdGpi8j3BX&9x9Pv!%V=tlT-y_F^5ajOD0bEz*ikx>| z(L+o|%V+FDdZA#EYcG5ifr^2nkD=|T%1;F^)2_V=*Z6b+Y{Bg++@{*LMEjC6q_O~Q z@k(7yUNay!6m7Tzz3#YS}$W`|r+roM5|vStlM!B_6shQ|zj zh-&YGVSz?yN7~nH3T(Dg4N;1`SLhtYF9Ed4dgp_MdeE|L6KVO)1J783;=SaInQ7&> ztN4a7BQ58Dy$1jdyIAW*AQNUo%YBgBRfUo{#lVdtv2=+uyBwpUV{iB&qUT1aAQ-bU z^_kIquJ+K8c3z!Xd^wf;rY^4%lU*>iSAh@U+bqEYl^N3VOJISjQ0_`CJL}LXe@Pn3Y(z zkZIRKY)R2xzGPGFc>hYM$LE9YSbebw&FcEqEQgr9f8=d_b(T?xo(F0CB9W9YYx9Tk z+5&AGfj_QTF>MoFCfhwS{YOIErhX+t3=dRlNmMYsIkp&>hAK8GAB#K=7skqE9nQ8^ z(9<`$;kYOHkQq1yDv83N_7Vfx@~{vce>7 z9}H#rxVJm}YWnYX)TJ2?MyK;>0NIM9`T*)aCvgRO1G3<9$}Za zHKvH&RhT(IDAB>O7v+BeX`E;_ z#|C%XyyPFZ$*jhMG%?IkZ~0A*;SFCS91tmXy`z~e*1PMsKU&Cq-Mk((x%0%wl^wq9 zF3*+?6xzJI^O92QpVb7ZtRW`kR0MhRd@I}z14sF8Is-25W$x*+iLOXB13TJ4O~9Hf z7Aua5)~JVWv(If{;RhN^-BpL_ zVt-~PW4sk%BgQHsY)0I%pv9JF46@=FWy@Atl~UA74Lt_y6AyM5qcs2J);J-jIA3nY zUs<)FS*3tJOsh^Mgiz-WAYCLPhFBm0mwe~JmJ&YG>g6j*6a=wI-^ zDbX(e-XSqjIxqXl%)g#@nxc;D5}H`V87|q!YpuT{95-&x@J@y^goQgLPXD7?OF2!M za8dIEPFm@oQ!yD@8 zQXv4=IgI^6jwQ+z?hmqz|Ipfiz_|Kn`&kDxD9K;;^2BJ%y^)u|$pDG~S&S!=OmN*!Sm%tCNAoKv9pXqKUnr`J2WMygH zil#rY9?l#QD_N^2MI_8Wj&B*NU3MnMnlWod5vlo#x3-->YinS!Woz-zW|pbt7q?Fq zzG%70rW5AFLpWDtr#H9v7E*5a;2Nl!_w(LLopTiUu*j8K zXQ+2?f^s4#eRB!=|Kwq1IUv%B%_RwEtqX8Ehi1G*`uJ-h-MPP}dW{hJ^>%XUV+j&8 zmDmq2>CCity_NMUP=WAEKk@2u=6k7|g>#97F1II4duJ?_A&PF%i#E5=RpH`2wr$Mz zPH$Bwz)=r7dnjN@ZS$H{{r;k!SZG(|*Q+tUxZV27p5J{bt*7{p9O4w%jF94ZoA*R6HT=`?d__RbdI0C%_$ONIt#d8CJs)A?Kmn z4EI+FbBH1=LfG8o-Z}X#5k9r|z>DgGz9jIJ1h)j@R;)6{7V|=?%Xw-&o#Xb|>Mn3++pB_X@1TL}amFe??Ph-?ufww6&;HO);dt zEyK)j-Vzv>I<*whL!33+5ET@*VR5CvDFe^#@7x~B>b*=f2YuT5plB}2;~WagSYla| zA^Bk={p4>P;O@-JsZp}cF(#%bQbtv<>Y$Y@or^bB$_iK?160YW@q2Rkh-R{c(VtP{ zL*%yka^|-Vpylp;_Z3vN3~-`zJ<2Eo`mf25B;MEp1&Rm3eo^&UYYO*IFH2V^tSJ|? zo-9gcV2oG`1!uFjClGd-EJM4~=QIrkP-c0gXEIJ&wX?;VV}vd4r0YYF8IKm3C26)@ zy%8&v2@9voVknRE+&f)V^QNNHDx~iFvZLi`NODJpZxzv=?Et~ z5EgqYQ#p)nISnZjB=Ddg0~~El-SlZNX_{mIyPEXYy^VMNm;~p=c`5LZ1X|- zh$c^NAL`qH6}lf+iV35hH-pcN((JQyEWEUG(-8E8KEIF_ZYhU|h>w`E4xi9hn9sNA zvXA)lkR-pla_3gRw3m-kGJfQRf2x5fIIiu@YLenw+h{WaoDO;I&Lu zoDlW{MIcJgz0_69QHcF|c$1a|^eA7?c|lHM5SuOzFv!GIeVS>3@L_syo7}DU`qhI2 z%^Qz#9DFQC-4HKui8T?p)=$3Z;IdPo`aIcAeXCwfAzUQo{iU#}$)R0hy`O`K#%_cN zKM=*{sCbOQIYOvxLt|G{46&?o{2%^SeKG3I3*gnG&EN=Ya%~h~Ecc6Wp<=aa$FA|x zvzHJ#7}IE`V}_BcG0r-Z@SXWPZ>aI39`1o&~uJh`-f1-BvKYu>H@RTsG_WW(8 zj0~0G^4{UT2NyaEzXaQrJR3Ik-B3vlb~+YhN0_D;`KsC!^Yl^oeR}z|V&o2^a3}V& z;9fvJ^W!s{l+ITqk>2Z_=({8rS6U#&y?k+l>A*Nt^2{YDsdp@(@>4)0I~*D z<+PF>T*8Qz5I*d1sGplWv{^;vgS3E#ZJgaCrN<@7$-s>uh^TQLZZGYsJ5e#`jAYm7 zcwsI*;GHVr(BXE$sjW_cG!ZTMRdrr)*dRChkXVte_ zg98Kv%XI!Q4f;+#HhMGe+Us0o#vaa{FAy zPN9_Dq7B)hnQXgqg!3-_KyZus~aOp?V_M%8G*-Z zR_(xX!hu*k7^>H4!aew>Yix`FaFm}JH&Hw}JX)o$fT;LKWai{#x!UBmT%LrXa#wql zAkbpk&%ITzkR-8as`SI+yrozOm?M{IqSBL`tPL&b_)IJ~XAr45*q(3VN$EXUK>xBS zn)l1V;O|7(^v4yKj{4_{;XdY~tJ0aWn^j(bp=p~mqObbKl}gs$<+fk>q?d#y_E0|+ zCQOBC!a1*5eKmoINdXvIyNhNH{o8VY09zxOT(DCGj zXCw(p>>0D^t?}1V{jEI9uHLxC|FDhMvQvHgHs?+}A>F4~L>LBAOMlBz>6z;BZq%As z0EXH+3Fes}&Dg36jg_UfT}~YMoD9~Jt0ElJR^!)=3m%YrgEw7WP64J)qw0w-E_E5#6y*H_`k`Qk77Go7(t3+F94O%4F~Wc_9-+VavM4V-)YMT&R)bxyc-FkEM3k@;SShs7_zX}F zc;HI!G=nH$6yL^5-BqXw@v0g)!+sGja!f<&OKBiI$6Z>rI9KZ> z(8w~`=ebL++;&pUM14b3dfKYrI4)ZPRdu1`-96x6bx}lcxSAGyA?d;TnW8Iht37qN zB1=-e#3Qn-aeRV{CQv=2!m&%ivif${-uOndp|fkc?ONvw3T4Jo?z~%=suv%MCS|G* zfH!r*n~t47yT4a?g3)l>rySeyLqtZjsorNS15XN{wZ1DG%FOyUNcBA@@#!iu!{ zZcOycO|V}RZ>L7hGDiUkFtqJSj!7Lc^{~NU;FP>#m?OBTm{dk=V%DDKr=Gss1@y3dkEB`hm=jAV_ka+u1j8u(gj767}ww4f> zTk!RVRM`SkL^=r>KN8TbhUE66hlE;C#CQvMG?ou&f!s~7L;ISG&Zj{;p zCbkUvopldB0&0nGLC?oFqI{X>OvDg#_e?7=Y<{a`{0rGL@qU!IDP{3a91q_Xm5{f= zGhHo21?h&Hl&WUt7?3nLVElRU4j{-o=wq&%&#!B8wEhCIZKH3?um+%bz728SN7ykv$=A^n*|DA4afjpRm4+lq1fh;mpaBmugk3baxe{`$gfxo%ATkExyw%Xm1`?+$URKEfBEe2?JpI}l!h z+pO6X{C9GcMSVgzYOvnghM0sc=Rml}lVC5K9jZK2N4bz^LUC0>wO}K8hSi{2+_dn zM2=$ao1QCt18Z97ZfT;N!Uh!>PM8#2Tyo_fi5rkZL*(L##QS)dyPaV0yX$5uZI{v? z@GNv4!={Sj#Jp!wdfjt*(7bD)oA9IELt13430A&ft0${s@rrc*hC;(1omyBesOXC! zIqA2@d{ShM5|gwK(4K1SMUo0^NTU4TBeb+(`!I-<-qcNW2m*-2dr;u0yf^t;+v4p} zdKyVr+6M%?!c;a7 zvciYBg82mRAIPT0o!$`#z^hpLA8AdK*_0IUPF+(_$lz|Z6^3m-Ki=YPU%DACpo)&( zH#Po~MdQy-y;%4BJeu~U&(ICGe`A#?5g$~}dQ98>u)$E!S+iJo5Zk`M0n;;k7PA0U znk!uf8Ri#6e|aPBrxSMuX@r~m!?wEkeS>o}x4*vl=H<6aNx$HvT1qlQdA(qB*45mN zroTz~S;sKQ2lOZYK=<>%qoFB>R#`932a@DqfFktceFjXXep?H?qRt(Yv8@wQ=~~`% z2Sb@e%QdG=M~i40bjb^1_URwmr*glGgXA_6SDv(n7*Hj$LS^3?(t!QN!BTLSMm6d+ z`T|$^4ohJI>xvff0v=`o|ME{Bwu3Y04s*|0T1NO?)m@Vg8CaQGTC z5QCMinAW^>&UV>cvqKK)mC;^{ZGl>R!Q`DRwUK`-swO*muFYJ|6ozT~hInIpG zD+lME5{c9<0qL@rjkT}Y4E&jWFmnWs1bsMJ%z%M_ZFG+rl@i?~Pjh8zqDQdp$lkLN zKjy)XkhoNV@+=72DGWj`j%k#Ns+9l}3r=k!3}mY}{RgnY;Cmvgl54ZvhmJ+K?W@DX%n?O=^M&34(Wxeyp9vlW zQ=VRU@=wk5NEq>^P>E}K$H6s~v&D$SBlYG8Z;7uxoFOCgRyv`-q(LB=9b_F23Cq72 zZ9qkho}9B$isyfjWs;===qhbe?~`Vg3B@q1-nGhNbIx3d<6#^5{kP}ReXJcRG&J#= zlAm=TJexVt-H#5tJ--jzj^E}E!WyHayzd9WIM*B<@i31NUF1`MHFDd$7f&wktJQpu6rGu ztZmAw&$bn`et<2`S{Vy#(NSnAFFlID$CME=AsB@O7AEKABe}_RVPJlU?nrq2Vv}^6 zA8UihbR1rGvYo#?QFujj*cD<-JgNmTOMv6zVfiI!-9k(WkU<0>sL!5M-_b9ERWZJp zk9{!sQqQS795K05-S(yB?x9dI%^bRVlhy6GgjAbuoML(XK0ge>5LBfqqKc4~_{fD{ zs)nmn9oW)TYXM|x>dhUqIHv63+6-JI%};G7D&x)Q#Wi9UNWu*3ye_YZij*6vfRuVali&V%Ks`ZVqg zNPs)t-h^0x*J1r0Rs3OEocLk6oTjF!`z6{7^YIqa1B9jG##(Vo)AgtQ=Up=@bTzk@ zCx4yA%0HYZ{WW04S#&mZ$pl&Fsk!6Q$%A0a0@@%lc@m&aM7EPXRW15!;12d6BEeMv zL6JmI$!RWldqB_xYP*~%H%h2Q4S6cAZwqWfI>cYgeIk#l535t=rAGafM-k!rXfS5~^J3)X^AUCLB|56v! zTDN7%0Ax#@QK@z;vLm|n93iAuh4^_F!^`tJ)yk%dnLw0k)GMY6vnMClJ`V_y325z) zvw#g3_hAO*mOM7+hRDN@mXAsem}ve6$TV`#nV@qTdggBe@Tf)awf?h%xAoA{KFGa3 z1qh)e7G+w@Re*<35D#gozmdmO$XsSdcwXwi!JQr5ZrjI1)_c`ZUus25SDaiq-@>5? zgy@ud6KuZk6uP9J;t^Q?AiO4qBrzL2$!(B$T;ya~+o^%as2tBnZ=79(sJ54q9kQFo9iu8j@(*HFF;oqfGMcEb^d- z`!@(y4ncS;nIR}T4QC$sgii>sn#Y6r!% zdndZ`z1M1e%JpgTND9aSZE3lU0gfK%;X+J#o&#`B;SXzufHSq)^zyuEUC`ZyA-V2S9QfH@%McA( zk;uW{boUdl4o?W(Gre)n^pl>YVyVNhQ(MUNIp}Ju)zm2n=`Q^)?e^rbQ#i~-5~pZR zGmA7$16^x~Y{$r%)=;SpUu65)XlXw~TIa&b7q4EsNxcJg7?5(hzo;n9dRH|V2~-AU zG#Kbm+?}6h&5M2;RbE80#r#rtmn35TDf$Lce=CMgYkJ2I0x*qbC=3s*Octh9;SQk& zu-|ZiUMlMNCdu`W*ff-J*Mkkdns-oIVI;|0?y;{YF=wTOI{E6F3?$ts&7yOwf+VnO zof;@oXS1h)Zi0U*e5-ut zSy|*0Jf1uD(qeG_u8BAm=G{ZQIw#@RtXdEJe`$a{4A288es?H>R_T;iEaD!~;Xb6e z|KY{7Q^m~i>FoBl5_LWO05d?$zvH*n_jSu_?P}b0HJ>5xEhxtMl$?1Lz1aC@e>6}U-RcuX!b8dxtGF(17;XJ_GasOw5oDlZm zx7Ghe^#F-!@l7elIDk0mTMh8R-fnO6$MR(pp`1M*rp&yRCWY9NBPx z)%spJ1dK3zxk;=QGF6Wnfiv2KL8Hj9W?4oT_26gqX5)BC0mObPfrT-n)f!gJjSbc^ zF7h5xFroK}@L9J9JZEr>0dYCD4xowyzTo#gZ^N)nDwmqoCFpuIa90xsC-u|J$;U%F zGhK0&YD7W8p${!2>Geu&CsNYr#E}FA9xy=`yp4cI>1#4gbk-DK#v&SOQps)S9(t9q zCtJ2-mEX;%lML25qBkb3=`{v5aHwtnlqQXS1?;gmeK&AjI@bp zM&OCM_xO%KaHWFKiFgt5M@(BWG%Dp|-HMR@kiKT$QHfs`eE#;0-Rj1md90MMM((X! zcLI&N6<>|d1|We&Fe5qr1hF8{0dOY4fk*IC=1@DDDt+HIskcYW-xfG-vQ2pnjF}%~Q&tQ;yus$Ed=oeg$n`X#2>G82Wzzc!~Z~Wlq0%1X1yQM96Lw zW`Cz5BAnOGs7PzF2(zh@Ipk&JM%)U>!IN)i_>_FH*{|wHx+5tk(G(IP^z>%GGLw|; znY9{1IbH0FbsFvyT26way7suvZ_;jMS28>l_3?Yp9g6{4K}?4|B|-*0=2mL>pF3Yc=kZ2WMZWgJ$qPz zm)$);f4m`3ePlTN2iLAG6|{0`5(%8Q-Da4CYXV_g>z_;J8K7!g2B;{lH{Bz)aX=*d z!I>SQ@BX!YX?S#Uakg2Q9kp?pZzv#KyX@fpvnr!`{&`MwAY+Hnz|+A_T^xU7L)Yg< zAmErAAVR-jUHQ-8GUR+@PKH#B%u-=z(GYOV*$$%g41& zZqq%{Y+Z39rFkSZ>Ls*&H=+2p$c`E^^rdf2e+x7Jf}>*a^V2DTBV->e_3s4uUZhX( zG(cp}m7DX302FcNN{Bq_ap>14ol{*}ED_}IQLindGuptqfd8{eD_aoh_ytP}*m>J` zZnr>TagYehQ?^X7xLTGXHwfgDvNWe8T2Afz6LZRnf|v6D`|#Bq!+AG|!s@~WvDRx= z-W3(k*hUUtOX$)WtqhA($wNr%j}t_P_`&cfw;T(a5&E~P>kw!vI*#&q4aqlmuHBx} z>Q>KM?VWlq*%5;9G`Nh7lWxp;U0Of(JjJvB$%z6sm-(3NUZc#<@O=^myq2o%T!0&K9*KP8%d;4CaZI9!_c-VYKXI$ z0E@(}rMutpwVF+kuf=OL)W5r#ine-eIkik2U{L@9Z+EQUTo2}RLf-&Lq)rGG9mB-c zK8m3kvNvQm(5fvryo6WqC5XYemIEKkjQqNk6 zzWWp2!*?}iI%Eo!c$z9=_)`Q5f5c_7&L|}*?k8nb`b}dW}{?D``URcLH@H8S@}x4$sBEB@#q<-VB_im6 zr4sdNicaPBBLI2VDi(i06AJ2z8egfbl(ZK$^~sXtW*PyD8rNU zD#5mBby3@4p#J?wcd2QS{LZsH(s5kfazz5YI|$0~H_;hNqUk1Ec~4@HIckFx;k^cclaqF&epLE6m-)9|m(jQLWLjmMVce zXpA(zUsoHA4d|xbESf_o-9cU~@WIlU1sQ2m>+L->DBz7W8Je#T7fqi+#7#re{O~V| zif%jGQl3hegqEnm2o+>j5@)D2dKz6K$fJFNoBA`E z;O1E{aVW*hL8pfRoQ9Pd&mqOM5Y9hKgH&*0J3v98i0)n=l6HG~W;(O4`_V}@Hvp?5 z|I9wSQz*=)`==L!euwh`7Ti=TMgHBju=&>9$5bix6zQhgUQsp(8OC$?cIb6^lamll zM)oB(7@?Im0e7XM1{7mAF%{*6`Xz*4AZeDO()lueRzQuYD-%X5Qfy|>S3tzI$Whl| zj54Wvdk>u_&evSbI!@1S)dIjSUR~%5OQ6fZ8AJy_t!>SI!_s6Mtb6nXVY|2KCqeu6 zo=I2;NO7A1!%%rxAirNA>~XG)IpxUknj(*|6Y!Bt?HbKVk<-A$%u8nbzDHeo<)K?) z6&hWVk4ZG;7@)o+EfTB-mllzGo@*946Rd$u4Q^Y5(JnH%W31iN?#CYpf09n%r{vug zB1#}o3!Sa=X_7E=n{}9emhDN|_OTYw*yUg=^<}DdIYb!)Z3I+3OIi4kd&kSsnt;X_NP$BFWYe@={6WZZel6^=^0oRJqxm%POM zB*mqXiuUj&J5c!B6{%s7qXyhoH%aF}s=--K3?uGFBz9k0CM}WF*duXBhKEn#RN?E- z*rb8Xw;}MK@CMJnz|txuSVVy`q^OB2$kAnBSm6+%lSa^}w5&s9>XsW(y(e4(Y(PS! zz6s%qWa(|a{+ie=tcy%RgNDSyIx3M1nFCW|cNxOD`w-wq)p_7|n4GqQ&obOUPp_A0 z*{p(r-&Fykjj?y5{;M%sq!BBRB3i`WmibBptpvpBVl3bQEz&R%cP7J-f%P@~;%O3u z1x)xU5=ctQZdCPlk4n!r#a9N+ho%$0kPJTkK`X4nkCC&GlhqF40@`XYw4bZCspTS8 zk25UNRIamxqZjt8C|nnfp|C2-*_>0V&o1NNNRBO~AsK-qtmXxw$p`BL{*&61i;1T# zuiXKilr6qM2>f;(2H;{{uI~8Ro-l?NeQ$V&3*L3IB3O2(!Y}aM)4OZCFJA~_(bMDW zs*(19nB5ZCp5u}w4<}-iE5-HU0AA?A#}gfy2VJ%a?{PLf z)~+rVcQUzhR$`XE*V69w6@ZuCWh0ft;RYIy&0Jgi?sDA{?hE&)Q5SFl= z(pd#E7ifs3Ys0X5Ny88QVU(&ceLPuNRho&IV#RPR;n1N(dSDV@igMpZ?O4uc%aJ3v z&3|Mcamm5pgafk8`8$U2u4LyvXZ)fmsEKJmfyGQV$d!!;#k7i>cr^wg zj8RJE)a`(VD}jx$D8~X2MkX4RhZ2d*hRBuyY7O+?jSA)TEjfRn`L8#g;JZjVqhjjCv}Wm z3zjuwQSVkYqd<-R0SY)46l^$319x$_ijc+a+{H^?Q|szZ)kCc!wl&yVk;l#9j3-X~ zKacmjGD|%@0fZxAPAx%oD2hy`)>sF_zKmAwQPZ5kb8v6C#Ye}8MNg(?dQB1TyXAmpak1ND`o+SgOwy8{}m@voG*dn!L4*gGg3lL(imVS939X z5|~QAXT(J4X_Jd&5j7E|LHn^vY5~%aPm3Wx*CN?%iN|!{SoYp*EK=w~Kl2V5-(v9P z)}>ZW(%6F$_d)%YKcjPt<5nqh$k+NxGAqDzKQxkXyB)QSYJeWvCa576C5G?H3d!xl z#QoexwZqg1+~Eq3x2Pnmfd5`DsFLlnnFss{@4e**Hl5_Glm#1Kcf%M4Q@}2o$Yg<6 ze3#T&FFr)7gOWYmzu5QBdJ#DYYW6LQkj~es_rtbB@%fXvGT(FMU zYu^a@_*-?-do0&!np|4Z-`!npnG=<&vfEg+4bb_P*JqjJJ`6wT6Q>^qg@a8Bq0}?w z3L`E!&L%QIqF`k4KjJtBRt-?s^J`kx0p>%m48(SDTzR{bOqvtQ zsk1-9GFDye1SGg*boJ#DZHwYd(oy0Eps^Og?2nJ-n5+^hWDmM@Ph{92PL9{?Hq?EO zV}U$c;vn@d$O(^&$|jVkXpoMniH^dA-IOQu5c(i6S4%G4YYvChBVdj0kXBytiP%QN z(JgF%Wt@!70{=vEHBthQpi6;sQP(gQ?kfaWa1kNAei)pe+)-+y70ipaEz}#jFkTAv zkz|GLiFSEexo6WJ)$fT`$W3s~qCtfD0evngSsZdA=$U1r3KKK{T0h?LcH>5rW;Nxd z{r-U3-QtD`i<94!)=5O3h0K*b6;%u|CG`t9_*UHl)+FI=t9L+5Rq!wG{_k&_ziAf5J@*%cOs0^fLM%0*V6sMSmAKV-Ys0h_!Ib0|u$b>B#V<9)7ib3 z+KG_^0c=;I-#R%OM^yqw{nAaOd_d9xFI8l}ax3$;hXGn^&R2}g=0|+2OdWPYr6rX| zx03c?sJW!>cUiq_uwb$y5BHx>T01a?EIbcB_@*lcrk_s~Rh7EHi$Jc_J+I5rCZK3Y z4jxx=vWzGVe;VeBNsI#9chaQOH&3o`TeYUdS)F05b#{4BozMM_U7TV62W04?^*QZK zVtB3GD}Q-z(s%-LCcs8es)#M|Y{qX=r{En4X0eyHv0OsRERZSq1_{qE3sY11GEYd`o;;I|`v-*#wwx`6r}9FqGscn!^nW>QKY@IImixpa8*b=d^26S0 zVvwrj*{T2(r)SJWDRIlMcjmwG=jb-&VoXSZmrA800d%#sI zwt8nt zgN}%qCdB^VxCwz*ka9(qL$0A)x(5FO#Q!{t@~^mi`uh=GbR6JxKL^PA>8ll1Awo9D zvh4t}Q=?h~@dr5L!qZ)YkE+pPk@X8|A&a9$_^>2J3ZKqlkX6K!%}u3nXefS^zKt7& zH*(jwc~VjU-pPtMLjR3<|2=;evSj(^qO8|o+I17yuuXzHE^-AY6Egz+JxW6|S2Siw zF-V7#;p}}QAzS7y#3oMQsY*uMjfv(OHu)GL-P@P;u)g&X1s=kMDdSnK09#u(n<}T2 zykwqgQjG%Q)`+ zZ}#22jMX>@F{FlPJ-vcWYOV57)aMK)JAeEW37ugs-`(y}J-j?-SLwVG>ILh7hJivU zs&Y)o-(+4YxIdL71Pw+kul{&C1P<^XWW*OBki@Ian(?9gH`Df-UxBMnzKAO|;w4Vc z@w%$VhXrlfDo*jmlKiJo!cjwF(FF`rDb5N<$^191Oh1o?Xcpp+(B`_?_K{y%PF=T4 ztV03g+;gI+-GxAwJh3FrQd%0&R@)}J?O*cjvuinY_{C}z| zph3Ue-afU=#++emO_oRcR#je)K>+{6H8=JeG3%}~pu~v!LMg{S@SI#FjP$oAalG@8 z?mOTAir-PhzmbUAh3Fp)^+K?|mQj?@mkwLnFtNq23ARe> zav6OEh|ES|P30`g$3U-#lWbZkT^9YzH-GI&bFSoRVzKAY$|sSFgiH@--11>fv4cb_hp7>7YR# zKjz=R9b&tK<@_sokKSI;lKRYT6shK-u2)#14xBAD-s`vyU!D5Hi(<gfmK?m`+GGW-Ohj0^hf)Id> z+;X0h;CU8hrhQz)Dym+mv$QYnOe~5$BcgDR`ek4Z_Tnpc9R(`w2DfYyB=Ux^+sOqF zzoj(R$w?<`6on9Koz%wouD*cFUD|0}{$a$(&Gj)IRIg4{KlBv6?={dBV)ZKkt_3ez zk^VtSSZq+{*kFKs1ev_#$Imrx$UdG8#WZZNq~36$ zZ?1^<0nNsnnST#T|KUVo_mJ=qLNDJcC1re_o$`E zRI}#IZWxM1YQ0FgxW{OLu$tUrMe!_-S=m=o>IaJW~g3A?G6 zY@QK#M;rD_$|N)EM53ps1w5JPs_$S=WOi;icziT!8F!SiCX5+)zyNoTUYA}=zDSrd z2uYqs*dg+4hG^6vM8}}13)r-sZQx{{dT>{oSeh7s2-hSO?MV6HI&#MXV?}#1f$Cb9 zt??~8JiJOnj+VMvO(tQ`2<9f5HVSHbP&1gfYwuuNpWohQ3`fHX8ll2G9QH^;C!EKT zD9nY4?Me}j)XTTcWWjmnA`8JOc~RvI*AQ)%ihgPKToG5&|I#EZGhln5SO+8~UJFcA z=Ww~MILBtGq(BCb403%mjO9%*GCVnj)-HFzoR1-L!o$ZQTO1WXHu?3|6RkNFAUG%b zgh|!J%A$NPT-#Nz*w^tsN`9;Z!4d#Ub54C z1qMMOpKBnRI)Zv^dvN=HuAZ$&78Xfv%9NqFQiu%~O!o-%SKJ$`kobZnfFEdxP9}D2 z_S7-;*V{D6(8JoI&+JTMi0Cu!VnhpB>m|1d9`k_Rf^tri(MDmt!q#I~2Rfa`my#HR zj9s+7Kq2mM6F~R5HK6gFBF-;0FuS$k%gEtUZ3_GFG4~xuuFNN%5inF^KQEXMaDsej zSA6l6(VvPWs8{6ND=Ox^PgZoZau)4D`DM z!kTKn19m4AEp)I(2R49oE`-6%`Mz?Pkzr@K;13M4~WsJ1ysxW7;*yo%a&nZD#__u$YdhAoPx0RqF8r8o6@fIPK3l%ypOVl;hnfulY(TF41q zmSYlfU{^`jL@;U()z#KxUb!VXO|M*%DpzZG(Hr`5A%=hi0SnX@x&W)OCMcvmAM;9# zEf5n!OF67b%;SZZiCn`Okmbc8T3{BJF1LwN-Rj`>}J`%G>q@l;84CuQ_auq2xY-dTWN5K6xik_ka~d+qQg%|JYwr z+X1#Es45o25VUR_oIc9Z`dOu{0Id0-JBpE)7CIglyISRPbKqu6R1CCS|LS_Ipc;3- z*G0#rl+ZQBR;O#!q_;A*dp-${)p#=wrZu?2{W48ul-S5WvP?kE+cp$c)fc{9-s=~` z{|6aoId7lljHd- zQAsMG*aU|)aX#P$BvauFC>BlJ-jDR!`=U`DDZawJX<2lh;KO9^xmh(>p2~?Y(k`b| zo4v}6*TV#P`wF{M$X7#1ieF!~q(0usR_VMZA1&-98B`?#a!Nx$)^7P#LYLI2E@x~)$!J^8c6SD&(T}TA z9#~}tHmjEDkEu2I?cW|_9u+>{JK2U3*-Mv01U#ugs*ws%E`7EbdV;N`LT=-J_CK+{8D2vA7tsi&y!D; zQreSCCY!ndlLIMX5kJ{_W9?vVYIDZ6$|Hz5o&K|3&*`T=D82`YBh1-RvEd89NhM>TE7`tyt|6mP2z zmRTNfch8mV@?OS&N9hXfG{gus#|i+*MK^3Q*EO6XaB$HpdPfQg>s!$9bVv!Pp`p7e z$v7)J-~>ik-D-wr03zvtm*aNCdizy$Z!P1Y{@nSHbE4Zz^^VUI{47ZH!$3RK(ygHE z3>HxrX88NXr)#*@ZEuE?b&x;8K&0p6`7tK&+<}#87OeS9Ca`9O0W{q&5? z0g*e!y2*VLr&EI#sUa1&OBbt3vi^yNMD?G805?ZY)#~3fR%1T9yLR(Raea+eUc&kl(MYfTRV=pof6VR2)|E@nRqBpeO?-QNMfm^8V2W=*vP; zR-dr1+-**S%M(gXAJ?N8!BXZRFJ+PYOivxrGJG7Au@eyP%`98Sq>OIxI%=XR$edGF z)AFn7eV-4|;R{WrZh{`{>89`7^W3PVZ%cO?xp>(GX^nms5$5J5KS=FHnd6{|1{LFD!gC8119zgLjUeP zaB8XD*dmhnuu#~fbV{v8esy1`DIQ>z<$77|8^BzC6>iupZB~2SnR*v;cB3BKhVn(q zI}db1l7+{U`1foH9?z?asf}#;kTB>0k2%E+={Z_7zD5I*J+Aio+%=tZOM&WqshN*9 zMHI+Z37yCETH&Y_LLsyz{lqaGPs2=*EsP9Y<#aA#mG4%J$CUCO<%?&X6u{RD1Wh|$ zrV>DmzmkqY@VqqjG%f0PeC}dhMu_FtFO#p_;hWpaHog>9%lf8m71D%j6Q9+rkfwAN z^Rx-|&sz!a7i3qQ)y>#*(PBjNTELg}$uib@8$4~2IUGq6dZU7+R@7EC#B+9Bw zNu0~Ijivt~j`MQT`3yGQ(}tVivM5d!nseH@F|Ol>OO()mpJu`Cdmf z_)^mni5ULM&@&(k`5k8OkhgS2bY}-yet5DX*HQ1gxwcU}>)))eqXyb`mxE0@wRytK zn|32yf9S$dOfHv!3ZKTO;8H^}DT7$-@a$U@G=R^6{CcszEXHvJW%tGp%wF%7=06x| zuOZEoDRmH&rdV{9-NvJJrB!4fNz|v1Do4f~O2F@@-xCp}V9wexx;|I&Ff{m~PR;zD z8pE+6`X;td!%qn;m}lFvZ#O%V012ot3$xZpM2@brkCp9N8WwjLC2ZkZHbV|-iGp~3 zN&~=rLKqn8e}>7nWr&pHgI(=)rtjOJQ8Yddb=fv3GrH&5mM+#T={a9=_wWl#_<1tD z2resKuDTV6ej6x>B*P&dvX#i;H(<(n=&jbNb*qywFiP>!L|iN~R{+5yW(EJZHrCmr zZwGw+IzW?p-}_yJYB<;c{l~ z<|g7*9cup)-%Z#&79djYVks1he*rG1n2_#xK7JDAlnRy(?iws#-X*X5{iwu~qoFiq z!DywH97F(pIEYJPc#v3OJnlP%h%32r1E#I!2>+7hpD2%GS`=DXG19UXfBD4?owR~1C1uP-A1>-JmEb`1AL~l6Pr=(>dR{mF z8)_K%BSgYsQX%dS`Q5_RtRDe+L0k{y77ZEk&rvm{K%=8t@tce-FeGtyaN0Bi&SW#s zqh_@Umg>0EB3wMD$whas{Dk@xGu8tULV7&2RIlRKfA<*GonS^bgVz&n%24#_7GNga zG*FW*-BKvYB}l$k^805&RYoXEo-@K=jYxkN!KO@2D$#etM?vh~J=3z*L;pOAJD1hm zY`t}hqBR-*(zL+(OwaUr4>&EuaCQZa*qk>ktZg&JiF_LREe=GF5jG+sCJuDZH5lh2 zJCS0Zd?9t9yzPut)`q=M1=2AV0P+*wA9znSEaykRt-`L?xjmYn>&S)fhc}2Y5%Ep&^f?*Udv&8$A|>``51&c@WgSI_H_M0Ja|p4 z5@skLt%mMSWfg>(jnbA7Z(%c=#=zj(w5=P+`-{J^ity(@FcjT0`3osIfZyVPOAF#X zR4fSZ)MRV_9s3#c(*wU2`}W~{A?sF_J9DfC@*COH@~x(i{>p~ilxO_}(bh*$Bx#A$ zf@-7Y!+2n~h$!FF9vJ-#-CthPo@r*_dd48~v?E~AA>X&x-Qhbi(Y>TLkmq^Z&zt>X z0ihR=s)g>|HbFTOfZVVoJ8H!|`1}w#+{gq7fIbo64ThXcsLVJ6Dr_ZMqA4S_w4saFNZdlkoO`=az=r!i!1-v&Q9q{C! z;(J=Cf`pE@Ym2COp%2Pl0j50h;jxsIafx4SQ>zWFI=h+Sxy16{imxE`c&`pPQysVi z=Zj2_bkHh8nqR2RHI(nJONjh>LMaz&8uK#}UhXMk2XkE|l14Syfs5u!qkW*>xWI#Gz{@i) zS7V4n2VyI?9$L@~4GNK$cmdBx*hIbwrr}rUk3^;CIlhU5X8>7dl0XcjD8{Z0FhK;y z05RsgLkO9DCsEw zUPTb*(G*ZXkyLSp9>K_Vo*wXL2+dxu4w81ISQBY!4?=G;c^+!4Ecjc1Bu>fmc;-Z)L}Y-U?LYD!QR}6kPZ#V@ zbVQLq;3LUgY65?5vP&mY`D*sn-O4=z2CPKFd$k5*PTq#-*2iBRFtckY|5^C9!Wxsk zM+(W$s?~6YOxd+T1`Ay=88!+{(=?qK3-C?;m*?lQ8vu`gN+OazWG|;Bv9mesIMLo( zDSB)@35*)K?^TdJ_1d2YNxLwc?~;#hS1u6_7;1}f8Z`0KmMa7PhGdXFq+9qcRaUf} zE|+AogL#m#bom_v-r`OK41q3JFZd!i21$xYHoh&X!wHM*E#|W6Z9=Kk2QjkJH`G1D zBCl)KZlOYttH<7w#As2@e#Vrs#sVvr!b??O$h`DjC z_Z&r~Rvqg)Eg&#hJC}}Qy@7IJ76@!DE(&7P#IoJc!@}+09Jn>CQj=$%V>mfP{7nUQ zM(M3$Q)hsm4(IZa`h&d+2pBF(&7O{I2qQ`}Xr1wq#51JTWFr8A%I$I6lX>uy%A8bYDOmqpKw$Qn8 z=z-aF0+5{pBKoB@>?Mh+YkPhRb`AaY08%5tQJmEv>rLWVdgN3OMnd8ip%v zfY8?CTGUSW8`KLambcXo6~YO8zSPVlqDJm2HOF*~9hVO&?r=e%!-J`P^tSO=mMMvv zYOsdynvCNK7{^cda`$ErO^3TQMEpT#5+m#oCk^(BDH7)^gY}9`h zBo*sGb7PB{+q_bvrqs8#)dfN?+;i)S5O{k{OtTDsIQyYA=0!OnPPyODgF5;3r6uee z9h^NMAfq&k>O63|3WoexcPwtSy;_B!nwd8Y0b70j3)s%wu+OeLTcI-Jw|YdgnYcbN zwkv-@Q=o~@-r_rx)jQ+5eJDw5g4f9s-p@$fGc%kBBE^&9f1P3Uv%#2zZn%N%^dTGi|jpOoQj__O@jyI_|tG%9|KGj95EM1Yp zmXy8mU#OR#7Kp8{FTZipH%dtkzhlPoWMA5}wtXYm8w28}I7K~;;`QMwXVSN@=l!KM zB7|It_Wx$*)^#88&C6tSN2vWAP-P_H_4X@OhiW8x?~ zJU^Q$)-(B+NSZ;|I+Y+FKL1bhq5Ok4;Nkg9b~k7zhs%O~WAbx=e26I$dp=(o^T!b3 z)@V&I5O8rF&V=>N7Mw3O_f5hZB8|6XN{&3c2{|^<675auRd4DY>2AGzy-Nz4FAv*w zW=`Aijk-B~#Cfo`2yZxHqMl_gTX-I1@%mOorE?wr=1CA_<}4}KO5$3lyeuK%sn7pd zeZAV8iMh4dHQni5F>qWMaB5zC&8~|Ai|EmQgbApyzugBsZK_je8o?><`!N-DIKjj| zAR1Xg4>>%!*9+_;O$-jqoSUJv#`8b8<#_1rqiEVsDoL|-KzZo?u*Gu$&qmK%s>bj2iJF#az z-6Gz&x7fim#51t2=g(P`pArrm*PShVrETr-j7b}?L>sLIdQ`6+Rt3rtD}Fuqmfk(T$~I1w>)&~jk`3PCJakc`({N`Q zat0RbhX)ihW2l}iLeOsR(BZ4DY0m4`*KdRJq-`>Ldz(J5ER{f?4Ado>sdUxE-{bZZ zeD<-_oYJ#l22o9tXOSJuIMT=}j97_8E8r_6HW`>T7cN`(Il@Dx^FNOmJULmXIEwRA z&`GKG@bQm{VutZk&VBls+MO-`Of;}ibm4B#fnvZIg=;zb5yV2c;lfr+8QpFO&d&Gcl@ZBX70-aT2TYI#s~w^b@kh-AG@fKw(o7 z`i>QBdEpVQU861E!HRkG6j8Ll$4{BlQ=_nbi~kcQwwT^HyTdTVz zIpcgBB+a$&SH45H;5mC{qdu1o)E>-*$5LzxO4G6X+%S;)bVx0{6jr$RP4G*p{uusu zN45{YQ832`B`+L{qm-*memb!^bs54$dkCPD?@p-Q`zXy}Y~V}-h>%S`OE_-;OkZ;p zJVsV?LUVx<2h3pWXMxEV@!BiwrN)?sH;EpknDhFHODCOAvKA=D{DIL8-7Gv_ffSSY zUav+*)X9;9tJU(RE2NbgI#U!*%gpb!AYex&t-XZEfucgW{s#eaISzSx()hxNM%V*V zOH@}q9k1Xbi8oRRvtH$$%zbG`%=?lTZye)GlGeC41`R+S?_TG%Up>$c%~Z**X{E4- zSUpIR1s!vSK^S})$UYP-z_TrRR`K3bQCVR4aIQ@rXJ*a178G6l*!k#LE_sU~a5%!7 z0UmYrHLs90j70tIlw{lprLhYti)i>dZx7hROJvqt^dghCO_ZKcAr)X&2euP+h8e%5 zpP0`mR~WJmvwD-=V~PVN*AfDSIS7@+0h9+(&s(x#V}?n%RkRnXy$UBsez~x9mu-)k z_{-d1k_uOJ(w8X%^Pz#koi1hJ;>}?E4pT!;dei<7A(BZKNpx?e&Q}QX-ax{7g`#&) zdLRve=VK3kMxZo_>sxg!!B2Ju`dur_-rf~=b;I^b8Z~Mph)Hp0`*~I90_l#D@7x5V4D^=qi38-izK!> zsYSP2yrcsLFqw}V$DrlckFlsD!8*u<6z;J{WxhyXMMD@{8mEpVv`&J9fF=w~tQNVC z0VS3n*1yw8D*i;*!wZR)?-?E)Se;=z;A4w&hP`7I0rbHF#@URK*2bM&IR;`udE)aE z%i;$cKjAFI&%(V7MIZ0%!=UTFDnD(3YdimjmUV+=tGNvW+sJwU?elI=t+I>Fl7jTD z#re&Vrr$_Q-#)Ql;h4QcQ!PXKnaVMdwA~hxMMp$)B$-aTygwmIWFc0Ia9j^~?d9Co zN_IbJa6gU~9H|-DZ@sab*|{@1YANE8C&V6wfVsZ?Da8Xg*wJK6HK;DN7#oBVj_gsa zf#N)pY^^AZ%VtABzgelMu&Jz$d-k-}$l}!esoJ`L&}?w-iKN$yU5z1tHa>C#X59rj@)33AU+P_dQ2A52Vu_?q@sp!wzC05|^HhM>X>DNr4KyP^6X(TKI^se3 zmZ_!x`On_zpm@MWK{Lt4$M9odUE_*PIPoi2SbCh^2M!;?b>a|;T^gZUsc1d-Vn9jp z$)rTSHj_-hFw7`M5Gq%q&>6CU$jIU1iIUEl-a!5bLwlG5%xX$Zq( z?6rc3R`kZDCND>wxS|NWXq_!B2EVQpX>wC6Wjpc`Y-p9356`zBZC#MsJxlqQJ1T41 z5}<-rbO&iY#31{VT$MGqtnXs!nFx9C8nez}P(g0*ZqzsyKirFKkBBs(z92cyedADP z2fCf)Djau& zgb%RUR&}~fFw!IN7`TYfMI+i+K#dfljg zFt!@nCQN@0bhX2VqWu4YoXe2+>Jd~u+rb3TxWg#V-ZBp>yp0^`ApACZmqaW?eRw{> z)xi%m!<*;tHi?;vvU~jIWaLsN8hFff`w%YT7*=PdV>992X5uJXXLmW|@ ztt5?`FNJlup(WaHG9MTZid~X&p7Gz#lRJsI*FY!O?@rFMhc=L#%Q--FcKTG7zo=;V zJt*1-`4x&yKz=WVQ!-Q3{CkwmAG*s+vIpo0ae#s^Nu4-!k-A#w6H%5Q5iXQI*Hes`}RE4u@NU-8DaQ>&X(3FJ=B(WL$ z3jjMF@BKA2E2&X-7C+4fHRzb2SCu@{W!KvZ!uqnE^nS@d%RIA$SNo#&G-99q zS!y6N&;~Z6ZnAXpLo=cULrEI!11tiZirc*y-H8ThIg^JI>86oOeQSr;3ety#50sd` zdb+&#-y;{d(NS?J};{1lB%*!%=%>dp1h6L|fDGDIGn-K;dO=-*qO0DQBRlU~sX*D1k6mMZ6qHlh0 z0yNdOxFrA(re=*eZQwhRWB0tfhtnMD?^&&sE$kCrmaDSprfV(@p&7Swy&q-uXIHZH zd*YAX-VKFgxI5%cxRF}3h5=|QkC8Q3L3Z?+f*(Bk9~r@Z31sOYKCU_P>aOsI(TV-{`IcfubLhg$X!z#gsg= zKa6^VXF;)ly7cHg1Y|{KddBL%X}u}7SgQ9DV1$(I33n}1@_i__0C!=x6B9Z%*wm5s zPdWEb$EU%$D9*UoOzA6tXYlCvgJX9bY903j5 z+0bG5%~3GAY0y4f=UU7CKJc(eow$D>kp+=%eLJHx5P|=6#e~WoM-ewx-J|0M@;{l& zvZ?;}b#&{ubMf23@#G5Hl@3ddIS&m^rzIhk*UgGPCPbe(s{zFTqv)-^ul1DR-Gc~@ z2}u;<`0w>0EQ6n!w+skK!^ZUq^_|p01BrM65!1iTJ*D zLllcd<;QbJvMWPZGAwuS-)uliX)zdufm7x)6KNTO;vju+e}cBvfUQf%_v}D#d(3T` zJCRl)oh-QuBR)yxB>`gT^P^jCRG|ScCx7GilkgF~F(V%@tUr9}jKfSUNqi zM)s5~1~%M4`*Fb1=LWvY1IRMw8IE|h0$-97>ce2w2XOqir!hCPuHMP}#(-O~4+HXW ztyDQ54>NhUaU}<(f!%#XG0MCDL3MReY!-rQqNa2@m19a(pGn*NX>Vvq-aQ*{?IX zH{X;*$###`gbtd~K|9>re+2gY%ce#Kad=xHpyhD$7jXVI%-}pC&gOdgZ{qH%=sVE{N zIAoP9rNzt|&+-ksyv+iT)1X7W8mX};5KFln+K(Jp2o%9?Wp-`e91pDdU`$*8pld@r z#*J$u4>L>NE)Vr7Sq*wv-D_iO(7OF)Uu2RF}3e1cm6bF z&~`|(eB7h{uZFv^?k?B9y1n;K;~v90F>mf$Y-kVyD+G;mFBW#K5Nym29nUf+o!n9Y zVMvEHq^&+tne&ZW*Jtkp0#p*znq!J4*_*95IE9JwJ*?(>>Xtu_u(wx7#ld?t}p2GHm#91h-BV4_g`w!)ia!u{hj-On zRsf|d{6|8HTr<|F{`!-*nq^HXqF9rrodMq)Wf4Kv1xwjB4@x|Fg5ia*Y)V@kaz1Ni zEC+uY_vXJW85IjG$n z-w)gH-KDE+Gqy>6Hkmv1xWnjHRc9lho6;r2q=}DeaDMD~KBt&4Ux}36sPae~EP#KL_`o z0~0q$Q!&wkS$TulXacdq*}_)f3kk}-`k$mN98y|_b^2pjV!#qE=vAzd>L_4B(OKi* zqruN#^)^i)CVbiQke)}Y1!`noxi5>*5{T3CjMAnkONBR8o)OB4TDLJrlhgjIukcC& z9EirHW&`5z>6v|*r2w6qJX3xT_6+f%TDOBNe<<%)8nuLeOaIe+RRvw^Vw2!lDsvKy zK7+c5RtM_U#7XVjp-1H!PmlBZhP~Ulwq+eHN+2+2xk-kuXks@S{O(yBfD1}(GhHW$ zlEs@)b`7VoqXThU2>k|=eS-hphP z;zfOyZLCv5mzOj#(@lVn(A+*_b~*@|s>$NvcQi58v&%QXy?2x(WmpPK>c;hz+o zyAPeP1&l-VI!uCs{NV{<#49j&AxQPpT&JZ_TN@cI2*2DC*S(8#voGV`sv46M5nx!- z%$4^Fw#0t)vnLEWD*ay@LECJD7L||>pqX^8G7HM^k=DG z0B7MV$s!iXD^l$AND0vsL6TEDDS?QDV#Dz4Tq_enZ6EQ`DlW(7Eg${CjpYLBL`FLe z*ApJh6Kl5)E#ogSSE0T)cGACAB}O*o4`Yu$)qt>*NzXU z7*8e2LYpA~%!H-Fno*9R8qU1kcc_Vg6&c6(Ser6MscykAl#W0#oaA$h9K~i$pdyZl z!<}-&{Q9*kYlK~EHk?iZBYmGEp1DxLjEu;sK@$7kcLWsuHy62&&ag&tAZQ+~}*Op!3E02G#}3UlMAiO7dDVY`|k z*o0-;XbNVOI&sENO9^QI42Y%1tWSA2sDMs@PH!&085Tvvui^d{|*XF_v^- z!tFQf2nyxu-`@vIy{Rd5DAK(oVXD9(cgX$VYIsYkA?-m=BT_W|y`>m&P!U=2YTJ;`sr}yyQ=i$;^7=hVei3()%$NJm6@cJ*d%RD4 zghPS8Imc|K&+-VdxA3yW{ifZ6oL&F3>~@RTEyxRGX=@Q;S>kqS{%ZwVsu*@0Bj#>p zB2;+qgUPDomtbkZa$^#;mLf*TwBSk!o`S1|HP(TtXkwZ3X*rJir6Zk_Bx569p{TJ9 z7Z)h9;o>Vjpd{Jh$U$Xdk`xbA*r5Qp*T9Aph{Yv=s~#OqB;Ixiwwk;e1qXH9q`m66 zaO6{<21g@*@_riZ^wzH(YPvpY9S{~qf3zX-@_&^p>n5$-2NDb=?hWWKjzw?1@XNRRzLL~`n}O3 zpvctx_6v$JNZ6VR(mkffOL=URLT__k>(0-3&3o29bKxJ0#wDlf(hRKg^o2&Ne*7k! zD5{T@skw?y5ga{A+@m>kF;;yht+r5f{n77eQu?i}@TKtZqGX@eZL_YEJiWf=rb^OT zmo?Nc>LRFi4np5wS+<}x1AcEw&pTx&n#zYw8xu=w&6_}y$eBrcPpiyMM){0^tGs#* z{>xyR_+Y92Bih);<<$(%_2Cir*v#UpL?9YmE=O0npBq-KSIV&nEYYS3Cf1~OIH*1&)ha@tyd_7!#mjTA=qwUz|Jw0 zd)v*_tfw1eMF&VcvvSD-IOr?LdXO?KsQU){^EDxNM0a7t&)ZHU&>qBil))W(1Ht}Z z03Lwwi&8fQNe1g`YoAr(u|=3zwAP|Xm;tF9Ix({vw(sGy?Qa^P(G|@(mPDFZWI7lFEfm{>emz{5aCwvzE45dXQLi6rs`l^!yu zLDv%YkHpPQ&pU~9u!#yh?M~!qcoggifsH>kGT25&G>5b`dozF5Yat1yw>68rPU4Rf zzYzk#m7=VQu(GONhm?Fq3i7uJ81OBdbiqkQ`7=HDgo73dM2K0@2w%>4(4b=bfw*rlX* zMUoQzVeNa!eL<#mO^}vscp3kx5ev$omN{WRDr(+!4N+JD9-60D|4qv20QKK8?gP8e z!{RI57`xL`lV8v9I;%mdn-2%0!A>q|$#M;><#L^>Kca!s zp)cdOq%>#S32WYVjIYNIiPrjy zx9cXaR8xr5cSOD|?sx;dD|hYx8Gm1$ddA)!N_vPbDL?+~Q|S(oyGPqH0)Np5A~>qK z%=P<6_GNX~tzht3BUUdn$Fd*(A8S`p&LqnixL$!$y`t}0-G7g>`+8U1&Y;*)3Q3er zT}UCZT!6SD9+P4AXCGO0ZCfx1>}@2;m}-LMWVs_(KU!B;8!bFOIT?5=JC4VzTVt2Jdt)mwGxR)nrHx&WwUR0i@ zT1!cYb@jH;I#G1vpu1K;@{_e^;2cI+a~+F03GYb``5VemT??xj?rQ8Vy2}S05>kCF zgHH(?Rl=&<_zF^*ntGa+=G#1bC-*PVCImUDY1|z5mSFpUw}4g81~}}-PtT1VUd57Q zw803=-LcvAI!%%}EUiz=+fojPf{!K-Apkj2SQ5+>LDGf@%_dTsgFXOl68inun7Z5V z;QV8G^K+Eno?K~{Djc8ZSA@Oxzy|ce6d6&$uy856Wgf!3x&A!X!ysd(>wZaDZM)J3fM$W-oZN7#SYpMsy6+)xf5 z7@9ffnGR5cO^JU-g#L@#0h?c15Z&Thv81=Hc8(j*C-IwUn4LjuUJGZpVu%9l8(^2Q+=}EX|-w+ng7%g+1k+wLTNb^b(mJ+ zV&Xy0^Df&pG#MZ;$NyLYVQC~_vS{4Kal$!>j~%S%?uIfQeKrD2nZPmVI)&rlH)&Qh z${|aW(7ZlZ>T{k#s@)>Z$sZr+N2uWkJT;wx4Wbvb?- z9xiZV$lxx!);&<=WL;NUzw>REEATsjA}LebEa;SsOJ>K&dVunkC}O37K_r6+lm}6+ z=ShJWe0VAPK(eL}D9z%P4$_)5)K*LDh8<25M|8YHehiA@ZO?mWt@_9dI<864-PdkW zsut+*4(*oKJTtC22pxmcCspKq;^_hpiKktf@`BNGD(fYtdtm7U%U#3QFk%H zzd1|t&i#gHw1w=A=y0E6EotOmQUUvAe4w_roWnq z>u6q(scP{Z{{^XZfo!bJI$_XEx4)B01P#ol%J*&QJ#0iVjgDs(`oK1PL-%^`x^8>t4srfu z^`xL6Rl*1=AyK+B5UHy|Ye-|>6p8u7o+xWuYqo=+;_zv&d{5RRqgg`Gne4_94t`f) z09=m=r6>z)s~hAxWzG9`O~gem2a578u#Y4F_?f#tXJ&(>t0XVgoTZs3(L}V+AtgmD zv>d0i(|k)0!aHULCO~^p3;;GpKh!fwcO0tBEfD6*wspKCvk=`K_vnr;ll(GJ^rsc0 zAsuf^ChIBLQ7c~1h09}47ZPaYJjxILw2pwVXKA!l8`k{XJ)m!!u$9bRessWTS+NFF zt!S^>GVN^WxfBbb%2P&X&BRkquXwQ>84sL&z%vouaq{nPB zO{IQ_sn0=ITw#+KmDEu!k3Du}!Tj6QN8avpaJnNyJUj^%k(XN?B&Xf_c2Sy9lDwGPTHV6X{UE;*FUbpVQ)*zrWPcC;qA1%~FVJq=2~ z9?ijyQ!$?9`)Wskc?NaJWmxdL;2u$@&bk_z$lO{*)8aP6mgC|II6bhplDVRuk<@MS z>E|P4>Bje#MY3O=8vH#gvQy!y#y)s#pQ**r(asDji23dW3@~Z$d4RIAl2f&etG6E*{Tk zzIK#a+YmgFJ==afQ#hXp)>HEa4epnC&9yI+o1k7=N3aREARO}04!ch3!=wP$Gj);x z7uK6XqKkDN-V}TDl(v1{u!??81Ix5!MNs!N4ZbG#U&i$oDER+|-pgYawnh~6`3Q(HC-e2<=vAyPHM3n&ib@V_$ zZ696`?r%4i%K2c}voHy9JZTHCkbl66h(zBf9D5^88Z&>;CMD7b+<*HuoR_#*kwUv~ zJjmVM7;>m5<9{>jDLUMIR+V%bx&VPlL+C-KY=|AfU|h_|WFmo&y`)1_^uCdUdfbdr zE4deGKvl1?MnYi0cnxi-nFB4FaVE6=f9Qz8ZEbg?U=`5Xx5@|+W-roP6R#+0>qqo2~Co=2{GwT`JasXK#ii$D`)m;U_5Wb1UJ&NLU(A6_7+LAM%9fs zkfLifDN9uYLwJGn$$z((GQc-a5!c$7n!9xQBaC+zU;fdiZh6azxwP(Pcye-Zi7h7d zFf1l{08+no^UC9~^O4L-K7dG?KzKbN4oC3x`Fk-7LPLp2#bUgl$9A>pO4>~3SM48y z$2!|H>_FI;BhZ3a|8u_hXFoesnBWpxrL!~w*e|&Ktl8%&iP@>Dmh;@O5o*pZ={V3D zd;1YlNrf7Wd_2O60qOw@KLA@&(=S|qJqpbsetlBDINY zp{AKixLxa=cw6sm#NR`4u=7J9(K)G*YGNZsUwwC6+^fJ)i60w6_|!NzUPAL7HQXxO zgkiDJ`V!BAI;NhmGPE8j$e9#t-J?UO7v&ML(hqGx8Y<~3JW8}mKyQ%y>o=+cNVFGa zzVj-TF5*zq3)cSHIGLT8+HTZx)Z4$mK(PHN80=fN@aBXZh?#qu`J<$(S zxQ7#L^&=LGeEVIcFP__)00SigsuK*heHPaHf@7`pG%saBA?IvB**$sRiN70_poo#>bsjV4p-khUF96n2^r z?#6l4q4W7{&_Fl}UqLKY=NS%Zx7b>)jDz113b6B628N70U-Z`oZRx!iun}b>C)?m*(@20UE9NXlOs4t3<#2LN%At@dkD=Jx*m zI!ckGp!H;zOlCoUgnz;7@NDB54y4h)FFW+{00H+Z*T!`1OwvRat0t1!Hj>!(-B?o& zQ9$t{-pu0@B>~^wp8;8N#`YX-{{k`!Gz;=d(Ay6StF?nl4p|iaQ8!XTz@?7x(Ohtf z>sVLuZ2vHzioob1rp51KRPF1a^OU_|K8^j&F+|s4Zd_NU8v1S7b1J7>41@{mwGYS( zYRD^u9=J_YDwXUz;uiI#F8Aj5zTAuNnUV@}GcyccTwZS;YCM8;b5`3;nDLnrN>#MW zU0#7^5NGfZdu-qaauU!_pFgh#zai|3Q`(N}bjqs*5+ZWx@9lRe6#lPrdOlXlv-#Ts zn$%)j;}GS|FzD7gzV4Z?eJ+R3Qf+Tq_ItyrDsWP7=|^ClHK_er4UGT*G@->!LZ(6p z3%rKX9w~#O1~_ztF#4~Hm8AH}J<51ag9(9}7~tU3h$w96aNjl|haY~K3IJty!2u%} zPf%P@9p1VMaXr|H=PrcLX+2CEVK z5fq_uPoDQXCMh}Z#sGAB2h5zeuFvr7S>tI(;N>U}hIHuGHI` zPrrjjyl@)0B+RTGYKM&v$WI;SNEn_n+snEr>)#2$#2OQwh2hJ#a~ME0WO_c+f%U*bytzsIqME>cn!b4eFKDT#-`Lu^dlUW88?tfsM# ztX`f0-LKgQ-apYd^CPcs=T+ADAWUBw5i?Xn=$#a?EDwtETF_{t_*%YQOXYPd$#dNW z@{4un(d>E)uk3+gCBq^Mw>{Uq+383CA3=5C`%c-cD9XkW?*{6A*2*r_lIf$p~p8@7l{-R!U4+{Y{gc6X513?9SW5KD4( zdojV0?In4}Y{$Yw|1C0RYIUY>Gz1*&B--qpI8*1*L%L2tYvFxkNAKW^KqnTkf%LM~ zt$Od*0=89|i{U7i-q;dO${YXy2AFDOCpvX0lgNYuoG+WQa*=xn)gU{VRwH#y2SyN1 z!aI36j%EfWI1h1WjTVt`M;*)%_RsuEm8E{fJPtWtv|^efi-o#By~?3a@#r-J^XyPf z!(#&h4TSefqO5C zcUnU1r=Xy~qwbmQUxd-`E1g7e)VeG|r=a^j!lkm=1i~tf194-r1u1AM3Db@@$`*Bg z4_0l{sS#_#nhGERD$+?fWu^fwO`__-o9-(T&YqU~(lU!M7jQ1bx?fQ?-6^U~DtSQ_ z%nH%J&W9;xu790jT)O zSe1#p2(sibZYoT9qU#eT_FAG!C()-CM_%O;>zcpAL(%x0D0?7YHK#f1FW~{W=sb{G zt)@HjCV4W8246s0;7(ef&##kwmN`Z`!PjR2EW{BhVxmzv|Bdh%6*z{5%T}T2)`C4y zP6c3N%%Jgf={KMz#ft76)OEfhJBQG$X{JmM+-BIqb5*^8Ngt*K`+yGKX<>+Bez{d= zk@_BdVw|r-$B`v|IU50+d+~EZYP3u!iGDQf3zlR=LEb=nabl8+4G8|BW+W$(IcM()V3SxzumIVh<~KM`dtOd3(j zSYU#xv#TLr43MLI5bkigw_#=am=HfY*t78OI!wzDLI79e43rG3b1Zn;D%YyWwh2)- z&iE9wF~~p-7u|hvAYRvS9aDK<-1t2?_^8l`Pa>nONGfB}VG<8#lZ4!$mS(8}Bw?vj zun@89{|=yXK)BkH%!&SVrzioo3dJlL(HwVudIM{ud1z!MnwNN)%BaaW*iP|5?`h|x za3$I=L%aH8A|5@4b+D88r`YEX{hT+NoGmGkHS@g^2NoC!eGlwi=bH6aWB~}3l_4I! zd~4Gze94m5aya4UG$SLh7r`IbF&bX!P94 z*n;E!(OCKfxZzDDf#KcXdk+)*cC7YO7N%6r-(3w^kLkEeBeJjI)L1&CgvnNR4!ygE zl!SZ$X68o*AQ(PyS_y=|e>Omav8*X%){qTQjuD_DMD*QTpp>e|Ij`pez#d`L%i5t< zb~w*=B-foV2a85kRW>_vQwtRmgXwf#J+Wm)n;2!tB;tiQliP5%%=umT;UhD8Mig(1#vZBQUo7rWVm?16PgNdk zH8lxT>WuSpEx}Lf`ZA#pIOg*$B+>C4O;31UaTC=f@Q(kwEOppTjl|zjGYAeBDjFuY z&YX*cdQ9w6$+95?z101eZAos_JL6qhBweCm89usa=+!Lr%uo*uvY2@s94tP+o8&v- zkmL4ayXspD&ZkY=`LJleZ<)?xVC1T2Z6y7!1&Sl98fq+Ok_c%#58s3t6rg4&UKX5O z|S|l^Whf1?pbR^f6f-zyTEN8-U+!%VV@P@qbQ$FpJ*QHU92RMEaRAtKCxZA z&zg?MT622U!N5nBO3%{yL*oaXahyjmF}M~ipLVX5#0l*=)>==G3I7ZI4BN@2h)-;2 zR$x|q57HPjtoIVTSf8^_XTc3&;csqzamr}8wpFbAP(54HR%g5Zol2YjI?MqnbumYi z$lN`F;CuUq#|oHJNn<}D6vQ2!728oQz+1{B!C{IxtrgS{*-5V|kjp?fmj$W^&S#cC7XL6c!Y1p{ zVN!mwmzx#Xx`cMJSi%(}hyE7ko`V$n#D27{3H4C6r$MUlNMw=@tNl^joDTzYn=H1L zbJxH&?!8R%ZUf!FlS&dROB8iWalz?1MfuaK-spDsd zKr`B;#jFpgcieMI*q^n}lcdt@Nq2T;99M0&O{WAU<_i!$z(ZKJlitCz{{p%V}3 z#i22xE^IXK1Yg6}y4^Tk-?L$tN8}#l%u|PsMzx!8+V)qzz}N?5d6!xtC`ar60FHle z(M6@0d`y~=7ZVuEcL)2){Il$FVhXs!q*qMF{Aub@|Iss+7eESEO zptR?>gH^7Sy$(!Igg9T8Kpw7CZk(OX4-Xm6iyCuelj%ySmQA$l2K+=<@UEfK7H>wB zyzFs(ZtNWuI2o}tz_k6MJ}~5V_nB%W`_@q>~lN78y5Q zGB2)25H=G0BTAK(u&2S#>Lt?*e}g^dY-#?1oFl9%U(Z_tlI~GiCQjQ5XYcYkC-zQB zV%Qmlc_AYIw-*^D>b7R8Njia&arQJzA$V>iCW=W z*aY`Lh$gA+^Q9P;yx-&P!mYIx9G5btm)fW#rmz5Jdh&~D9K^;;G*>fc;r?(^mfB8) zythlaB1<{wI?&c{{KRi|HN8-y9OHmjl`J%D< zbFEfMV?ng^{}oSY<2ljk8LvwadgeTs8ZlIo`%6c(40&X*Nzmo^y=&NM1@5mNaXI!q z#nd&ue+9}1p=0}S9-4QDB2!r~g0H|-p80mascT#gIWj9;hliG1k`Q^iIm<57mM=Tt z1s-*(q1_?uH2r(3J0U@7ZNCfnRn|e)9;o{8DI*yrN-~$SP5cRw(>Fxk$-W#9NiRyA zg265e-Ay8TjVJ@nrWHUi)6xZsFUHYNU-7*Q>tt&70R{!-m$r+3Z&#xYQqnhmC1TG- z-wYN+X?8rkmL?|y(GbVln+L&V0RzXksyhx>*3Aqa@Nn__g1$PEF)=>k0G=Ym`!WP= zG`2X`s;F*eM%S<&wI6qJ|{_2j@#I@3%S(}P&Iq8R%fE9)E{# z8ITQ(S6%)j&B`34Kl<3YmcNi^u}o?|VQGTz5nPe&Z?Dur-T(&5K^B;=g+Hr8(jP4y zk_YWqO~)!UALBy7tM_SZHZY_l%T(QE93HA_Zn{x}CwA9ue zk83XgFe^fv{Xv>e&luxI*G?c!bgOEF0O2~BvC;{8CAE_OQlMJWi4FF!;w=%+1OliC zZ`+lX3yPqvKd>Ggq#V1IiTHQ2hu7veDX!FzdEDV82}{Ael#2wmX(>Mv_cUKO zG7?1H1&VA;<6y_X+Y1G)rZT+>@tPIuwKP~ohQ3WZ?Q%XkYjm}*sR0!-h3U-MC1Pp; zteD;J*%H&PrWmIxtmsY3G}7OA2y9G#P<%!nU&ds=BBJWiqa)E0nA?h`komSlaeac!_8Z(0;Nw;me(;< zU>V)3Fu=b?io|sj>;^naqD>GDA=<7lsN_-d55~U7Nb;P-bxsqf{J31WMMTHAXz9Fy zlwAH^B|0eBEh!Nj8eT{+1S$LOZgki%OA_W>7@mxI`ExyqZ2f#rJWILm79e}*uV5DS zk|)Y4_|ONXEZYSWJBf$cqRigzFE-9asxG3X5aJN8*PKIrs+L6bEoCfG3p5b zx``?=lUDTT1A155lITns|L5I&1`wx`0Tw!{w zu%s>gSN}JA?t)p&NAvr-e{lypN;W|0YU1U>2YdAD-4GcMlOYuj{<((JhL_@_C>j*R zV>jQ)wvoiweaNL&!~Zd9RDTznYWYsT?D%C)f`9bh&wtR(<$G4^7e=IWzksH2XXXDJ zE2N-A)sNjWz5;&pC4E9U>5UaZtA8R*zOd&zxX-HO0fKZ?K z=4g!1=S~1n0f`=1id?`Bt*5PvG^FfI4jwdt)GSDNDLH$=cbn6v-47)@r6Ks6+5yqR`b31bFHJsN^7s%W}*2}5=|;sBLsSV6gj z-V0r>e%vRFI+7dWvlKGvF?;k5Vt8BXt|shY!w8cTmwnm7>C<$|fkN07&b#mXe-brP zeZ;Jwm2|ZH&~?*CQXK65H(+c-O@x0Lza%3nPf4c~S)R^W52V{APuiI=^qMqlspP;& zXrVWVxzR_09~EcS^?caxj2!(op42zr>ae>`=AH8YnNI>y+El2s`eW7qyd#x~GoaM9 z4aoKRxIe|19S9>$y0po9c{fXnYGTeSRl6u4pwixxF^oNjZJNAo}L*2Sg}%H*jj5D8M13XX|lWe`!Wa|x);$d(=G zQ6!^J^sneTWOMCS(Mux?4H;0O(3P_)V9fr`%MFZ zIabK6NRj5(MU&@ybP2NN1E(4IJp8tmcj0)5SgL9)8s7reamIJVDW(Q?BGjq~)4u$Z5P*}h=A&lbWoaq|{uiorYQ0X&?} zrP?$iRSIb?az*^?M=YtCxns}ch^r8M^Ycm*)c9O;hl)}onJ+0JtfkbToru=wQD~== z=ViR5@j`*XzMu1j;4g#gk`ITweK1t9!wP7_S&^`5op5wd9cf@_TOYRPBH1h;UQBaH zI9q4>P~3B5g8x=$%DDBpmug-B%Ec%`eQ+?}(#-U??jb61Efk&GoPvaFUw-M6kKo;$ z(d1oe{YOfJ+BtT}$DZ&2w1NOiM;pzY(3>p2%xm*Pc=A7kC>;43Gw8sr^?wdPkXtL9 z3vA<$#hYAMV6iP~5vMkoz!u&;aAIe~7{+DZ zX>gm-`)rQXx2MfbIBNTR#0eQEYn1ml+c2Y?R7KPc-24YyVX|W&d&i9e;=Vh89ye5h zIH+70-Ova5h2Do+in+v zb49h|Hv;?Vo;}Gw;KYOPl@Q3hW@oK4M~vr42xqCKERE!r*GK)8OM2yi9ppGI=WWz7VU4FoF?Qg;ij-t@*<` zQg&!Q&BXv1<7pm9KX%EJJERIi<5(fcc~Li)%hT5f=9Kf6>@3*EwL@TzIN+@}hc>-= z+V~&qIt2))l+vid@@0gbDF5h|o78{rZD30XVXveU&(du7DQW`d zKP{ma+*-^8;J_kmP~m=r1MAOIcRt~w}4&!*=roy5w#Sh5|8>=#`VhI4W@jVZ9VV~ z5HS1)d5i&I6e+IADZ74Fm#8;M)b7}}nI|@fI}}1^dA9n?7@em4jLdNUmJG%W%G)~$ ztMdF@tKABDeUj~;s+!l4M+6JEy8B}wZh^EYsd5q|uvffQE7ee^Wwzm}?S)V&aGZas zQd&y^fA^>2^*i1(NlGCl=0bn9c?<}Cs3pn@F)xzgmDRX9m2uboPy*_&K9^5&E3ZNp zZl`O`LMX{`1R}mp7|uAvtY-40WMDitZuA;5Z0zyG7dZZ}^XxoVNl05=nV=R7PjVk| z|0Akhkl`Ug=l6p!_WN7in*J?aeFBK&u3r=R#7Q zWp0gSw2>ehY_jE1QQ+N<%39iyMKnx&qoNlz5NQ4PTp6Qn7rv=z>(~+*DyL62tqH5h#dY#HVDGIXk>eL5G;*%1U&urW^EH|M^7wG>s5vU!i?5>2jwH89v7JzzoXm(mG@%1a zri50{iB>vOpa#1bqNX9}#HrOl0?QsnlOw+pm*fK}a0zyGoN}PkL{1KC-SCC@1NQ+BdX@sIHIpxswP^9{Mo7?@ zAGWZ(X!>LRZyr}AH+<3pMv1~wXBV_S&i=pK=|5h& z9OPgvkx&tN*+CR5-Z^y*>WA&`pE}vTFjsG_k+HzpH-7Vl__yPbLCIA}S%mKJ~ttLxZFW5~4(vc<)oPeD&sEn&N>R!@kSfoP>+9wjY_9+x{h z)ap(0UTUI5$`~<{%R%Gj*g;mw01yNPSloPchV^> zcfLxj&YaZDVpl{MSM;mXtqYc!BT>z`90l+0&^bhnoywUgv7^s z@AXgMBImU48EQ{zpD)Yc>?;B*Nqvxhb48-L5oiU67wZo+^Fk-66&??ep*lpp*lm1# zUdw;}VP9n35;QRQVFE!&BAK6H!(3GJRB@C|Lw~p+VkQyfZ`D7J--h(dd~pMt|Ne)e z?ZQ#{OJZj@RFiYVAncT(M7szdQ`aO7AVne%EDPO}N9#{lwp7>KEfug~^b7xYZn8Ec z_5jdhq$HD4h^S0sUFPX)sY>6~u#|3&N5Y8k4n`0dBm}2NYD0f6g(FH6(hCYjGBm6& zYX1E(RjSjJY{@*|B%b><3o3HPv=%wVj3?ZrMzSo@0R~JlBJUFk5r-5f1P5`#Ac`-c zifUe4#1w=j_R#B@z^oAZ3c^2N#ZU~gVFB`_;ae{{?uSv?RK4)sI%AI*q!Nl2n9E;x zEX`Gonp+Cl1j(whNCam5a!-{kk2nK7X6x9BNf1QBZYm%oy^7>YnA(jUwU7q@BtgKg z*;G^*h=p`KAO*Kig=M4~+TYO_S6pJYda#>TmOI@IMz#iUYj#~)p_Q`HK7)pL|8*}N zi*ANf5!W+oQ13Xq+>@y$1!_AFNJ%<)@WrJ%>nHsjFQePQ6PxXLD~K-fBnrwruT{mB zd3xDCis?SW0#ZzQ=@!3UW=62#|I#M+f;43S-z$Jx%(eKBmBpcQ-KQ=HCGK}FeC9u% zg6#z-#C6Zm=Vex^XjTNfv{00)00&rQ-P^Q+Xs8+U04DxmmP5L*9AHrU(tHxX4@9YN z`rs`Ye6X#vHU*wv=N={JY5h)hC%Q3C%@hP?fCQM443&u(57zvqnCxa)a%9q`!QOnV zNu9^?NV_w08IWlf1kt2f>w0Z%w2-jC$^dV0LA0kr-(2$pr>;ZYZ*xLpodD=UslQyp z&Rg$kgtLWAy)gwduYS7`>$#@Q=r>+PL?)zu&ZjORX8gE`_84=x_L{Z3j z?=$!ttnCScurDbHCRsP9S+iVuXkHLHKc&qUdJ-EFdCGF2J8_TP{?>b3J*q|jYtnXl z;Gh(W>)A0GyG(ul7)4JfS!>k4mJpD72(mQA)}O{a60C)u>1Kn|Icj~q$s+8>kb|y( zUkVLtC?m&d=W5upXr=boiM@wzlMIn4mVxQwW6L#97>N+t0!Imw3{oJsvf`4RNMH3{ z$3^~F86hL6t$3QiYEdeaf-8+zN7xsl7Xo`8<8~=qo^xPG^|6)G%2WU^K+wOpY26^E z*_*0698@`=j=YTXQB6S`aIl4{QIjo^m@)G~vdAT2R+qG^)<>q`z8x7{I8ME|e%`fq zuxS&x+B$FJT;TX=UE{V63S)I{5#lyvHY&EBkIYrSy$<;g7SIazAl}o8j~ba6nZrWbH>b zNk$>|Wr8otNv6A{>)?{nWNSVv>gkgb1 zJgw&pSysUa2nxDr*)lwm%42C4 z&zhbON(3>BQr0lVL~Q4_CKS48jEMVCj?30MQx?*iSI)d2U1XO5Iqy?;-qC4-Ax_1j z7CqPEnoT90amm=lFCRJ5q_7~pBjAju`T<~O$|%CzP(rRiNnvtZ%D*CJH$p%CSe#$Sb;9SwA6$2h8!lT$ zQc%>LWA=L*!Qczq3IU_3S*CfB58Zb_fKfkKPaG7K9l>jGMoXyy-y%@6mo73H1l0T* znWh^3&(r#q=Nlf%JbC*AqZqBM&g>}}mqZX$Xx9?A+zwsgX?)}d(CNmG$x(<#$Jbc- zc|0Ij ziQd$wnQVn$kirRG29_UN_QEgaM~OA;qUO~t{m4eV05QL+jagMEi2`BV>*xM@22zx7 zbgNBhh3k3;HrD!PQcqF1$)1V9;(i#^=9iwATD$AfhZQLAQiD9*nk9U?kmcxusQXN( zcI0q8hR;1Tv_d*B0Xi|jETvBfzFsxG(E`U{Z(v?9>m)}&opjL#`jNKx_>2Mu1u|qz zjXQk6OIV&L>r813v;#ONE818Y{fl-l`1RC&me~#n>Tqvh4b3=I9R(gZez4(Wc^<}; zRVJ`Q*PV*LoWQ_-E<^c5cL)|(&j&%GuJ6`XD1i0$5Fy~{7UrH z{IZ@nsBC@7u==lvSSE$GgQh?ZAL5U^6H~6B2)(T7_qKU=3IRQZIxo?Z z$sJ>V_C2F~ei2TPD1wP86^?q1&@gN@3XJdyz~h!6u45hTQzd4Sen;gqcBd2u#Lff_ zfD5|@2AUPHpIwPr+%%>;uk)bE-xD95>ad^H0=RX}Lsg z0Y9_IeLA>{+!@*6G!Sug_DjYGH^~n1LN*!t_ZHDh6>vXbdbMs~OK^M!(~@Vc2lEc+ zmq}jMs_-zcZrcgTDj)8@{iNa&5yZaqF96&-mZt7(hoRDMTVpXzX%M zMKokOY<jinLbhA$RYX0kATyRQ8I`Q+w|& z9lqEJr6lXR0h3I1pFXXr*cm$Y${<6uDzv_F!iFO(YJ9Ct?IMO7FG(?0>%r)oYuwIF zxY0<$ooLvFxHg2@+K%c4VN3e7ado3^Fm9qxrT!#wm1RE37pI~_fDjTF_te6y z?Li7Q*Y=++H6@;QPX{Vu{l`kR`F_9Cd1|e%sKELYKoF)_E%~`X{Lrv1DIKkva<}uR zKNY9s5)fP#do3fmiX@ZO{WEIhuigVZ6K2Kst5GPp%x4rAoNjx{25gRbW%}5u|dq2A0zruR1G^H=)Os53AeZl*NYzI z#l9fmL5}=lv`2KNw*eQ_G~+9-*J~-kCcx{!by?}>;iBnAQsfRB^0CYeo0=V3 zlT@SNARF-C{U~tUD)yHmb92MlE2Gd}FCP~5lLe163I-VYOx*(+#

yAT{E%j(4v}vE+zi-xoC5I*v(mvf6QT>J?2$o;zCShtn0uQ<5Vt173E&(~t@58Y zC6<51+jOIyH%lJz>nv`%XjeP!y}P?$Q5Hh=j3@h6iY9Ei6?=ujZ;Ema9t0S!?G)Le z!wQ~wSErYXS(T77xZf)^&87%$W_V@xZHm0g+e3Qa>?+C`lP?Hv!sbMH2Ou#EGL?k8 zS8Ql4cl|0J@v%dkM+bi&)OZ$-S8WkD!slm7hZYQdu*LflUoI|kjiiHq48d0H~#>@&M%d;Ji@+_cs&|8s5HFSb0irc z1sxAq5X*Oo3leOjoZbQx=L<<U$Rc55 z?hfrt$SHEg?(>Y@XuQv*hG@5Q9qN$`dv>biS z-1h*k`=e)`WTsPgBX~+3lmwA)#+4NcY8ls0@QxDXp0UdYP|>5%H;kb0?W%QV|5Yd% zPR4XdDo1E-(|BzO#_aBQi%yF>0+kQhHOrk+#SWs#PBvzg4Zr%FU`4hf+XI-NrPvH< z1pL-?Ij{qP7kCSuRDRY7*M{3`ikzN>Y3KzP7UF?Lq;YjBb8U<$d8j4z`8gS*jdV z0_^7yMDpJ*T@kyQ%vB!WKqrIlVVex+cO`>hw{pCNpexSJ9F{yJ1vfzX8F=gqL;vB9 z1yxzz$%&ATYC^U|56KmGVcM3I6dPyeJ*|fb1KQ$Vqwx7`2y-utRb2)@sN0MC+$-8 zJpQzUNP8n+n4YGAeOln43LzE8-+?m<3CmFg)WH-Z_wa*RB`Y;RMj?K zw)YtfM^y|D@Mv+emBe^wm$JINpDLG9xO+|R|Gb=v z7|8A-BuDZ(YB5UT8lZW5mIMgBY|D}tFpvbozD`Z&DhZ4S09&Cb%Bf*SboY?Y&(App zQial>DIo3ev{`iT;fyA0>vxupWC8%==)vt9(|j5ri|q;v;}yXSRlFfN78@IUn=vEj zHruQeDEmD$4om7X!*p}(fAsz(#Z$5|^b}7T8G|-E-&ElF5<7JocqSAEO1-t8A<)C^D)QGH{{|m7s)OFYL@l*Mvz~3iIh_ZM;zc>z+_yx%R4JAw7wT&HZdUu~ zqWNn9v~I@fNfGZg5dwhAD`VUG19XGl7OplfN~I%XNF6OWEN4X+ys^6?&! zH^4?o16l&lVU>%c634eH<-&d`Yq^~b5x!z1wdQqSAWb4(fpT4=DjZIB1d2Ayv(%Pd zmNYj|Jt)|4ua!VSI90-UOqDPmtM&5c0HWYVoA6%U%{NJ8&b;gV^KV^^?ZTw&i1Dq& zYYoDy`cXC&5;rK3)n!NyBq2B+?zDm@kgal;1*oTSSwji_Nu!F!$_R|;UUl#w-*Tcv zV!Ehvc#M7Rt%0n(1sKxuQ(<2U;WhhGqevd<8fXm(2~*qhO5TM?=e2mKH}R-f({N#| zL4;#t?QL(|r*EdKQoaSy1Lm7+MznASy2}p1ir#e;J=iiJ2%W4i;(dLv^F}`r@ z#Qr|qnbdGbGT!eEtuewtVcG$=8G5h+UP={vLRlt&6(rsB^CHr2vqRXa@ODTeCJ~RN zGROC&=df~-MP33NSCJQd(#~%Y=HWq5ap`+fCcWkBH* zC6Y@y!@>C)tq8sI92J@p2w_d5Tx06L#3-z3t69eh=}23l2hRd%=$^||4m4sP74d!J zLTtpf+>jKy4D7b~r~r_FJOp<-JOVRWhq{ArX2^+1(+9TZfb%MXy-8jD@gT#SK`Q%o zbr+)PhL#&@7mL%7 zdWdRdoPF3Jnve33bX7x{ftOCs336k^97xK+ogQnVkSR2RZ^T#~zNQeb_Ra?CJz*>2-& zI=sBh5QD;GO3!n^*B-x+BhMtYeqUXD{}6t?hXwtZSQh{E1tvFPTVnWDOB^W{plSY# zLc?%Rj>*Y1bI;swbP_oG|7#nex=~E@Sn(4oODVIdYPZE{TFH95t;hhT56RkW`ZQk) z%*sT0Ec^I=ZrgdbYS-WQyqw4XI%)$eNGOfVlWVEDq7dnN@hrU0??*qctLR$UB8y96 zSxvMVr&@gt8Z3ryqBt-`KjL5^?RmgZ-7Qb+!X!;9RD5CRF1Sm8T|0LrNkaZ)FEf}K z^!8-+^PhBd+W!&BZRWEG`oS`sjVOx~dCw~1-4R@E^87BoOGf{;h`QJQ-VZPo8)VDH zl}x1Cg@gumz%n_x=kVI_k_LJB8jA4;e$x8&*`u2?5u9E^c4aL~yF9)JVy&TGfR7g# z?%iHy_F8iE0ru|tP?KMr?#_f?@2Wr1W&or$cdOaK&=SmF&sC`*t>wCZK0!0W2>yiq zYBNS&2j&{ z?zXv&vpr`#UC9E-!&A+o%(>_ySb9|En-Uuc)13_4diYf+&G{F-L&9(SitfYh)S;4X zv(A*YC|K*57~^iX9gX1GL+;R~n&O5_EbvZbmf}Sbn$n*TbpWCW~lLeqHvo7Mr zR8dTB0$0?^(bw3g8gApFd$21itO1cLDAfAJUSQJl$2^P&}nt6LNt$A58I726A+cIxH1BD&xy&6;}Sr1XCXDUV?{d6{-a<`o3+g` z1J%P$$4Pv_VQaIOa{c^J5X@dyRnFNnuX{nu3vmOk@n|aezy|Osz;^v|D&xtwc)7KX zM)17rSy)dovc}zW<5cUmXeZIXZmc(y{BS5LYI9?Bz0}KaZsfaalu9rhY*hKhEmz8S zqy}ZiAAs=!))ff~0yD|I(L#-8IY&m@OK>l3N~f%vxFb;iL1j9w+w})m_|>2qR)n?q zW_A!BvSX)%yA1=Ra=y9=*VFb{o(z+JBo(%Q^g<_XB@PDKiWbZ@hy~od)Ljqprfu3r z6U1V|un$3^-4XC0rx6e|#1!vOiTQRd?Ihz0lt0gui#TkWeRJ;dQ+q&|k-~a$ zyRf58lRS02D-2WQYP54f56x6kmR`}pd8I5%IX-frvAfHFIigc5EUNzagGN^IviR^t zw0TECS8^I>h$d+{Ejk2WQcx}-U>WMlb^iBq3=VAUsKBsEdA|xU{<#mJ>)YYd;#$%Q zGwqOrjn=qPh}Nu|6rIdR)v-O~CL~`oj$Lq9fm71Rc2~4|O9MmC7z2v^!lf zs*+1-+b4u)GM#wkpv8Yh*mz}#tS1%wm^JO>n|^&jnpdQnuRo6ko^Z3_Brkj8dpa=Yk+2fNgidJVYzk9rJj5uy2NpmkJTeURwP<*TOqo^*VSO69` zRf3E?RhR?_%31vnV>&f1$spJ2Uhh71ulr0ep>LAOB*ujvC@Cd*i`y&jTQ*kmW zQbY8iLl$uJ0@ITO;DtaS)`6V#x#n^ft#l`E(b#NT7`vP4j z2?IfS?4eZ~MBvX=5dCTI4^A>H$iuoc5K;QE`)AZD8Vlt)%X8BlB9=R7uBab2CCEg> zz-zR~+*#aJ4WQb8X+V;sruu6ML2U_4cuyc0r#Om++}gqOL>~8zTC+&V`}k|F)F`w-!BEoMPuw5Ugqp zoGJPkl&I^6_ixagO5?$uye7-t zPX0A+JT_(*(LhW?{`9$h5qDDv>k7Rzso$pTY9oS#mZ{&)a+lELYj-2{yqn1RLP>Me z=pcP~B8v4IZc+bKXp|2*m6P19UGOV|%FNn*?`^9l_@O)Ikj+mQ3_T`FXU+tTQ{?J= zR=GohlG{&awr@zsYXuPfq!(*_syH=xOFxkF1*xq6hOtIek0H(h^W#I=PVqJ2(F7vQ zFvjS44OoeOSj0{Kgy!BMIGnf;3n>C%C;mQPk^liqEDhR{P7-2bcPQ@?+!7v}lBA>| z;lhrdPyA(?1aLAM80UX*i=seT2`z*^Z z@eB$sBGgmi$4H?=&0Y;5j#PHpXT>CHUw=Y3(I}UeMKi%}PYNy3B@(h(i&)HOPp>kc|O7$`WE34@3 z7LAqj!ta2ul!Lz}2kvn_RoMIWjRNIzN;chA+5^D|B;JE-#-p6v_7b~m;kT#&yJl0> zW%ei?K65(jhivOZenuo=K2Q-X*~b6EM9d0FONB9SWF+n}(T;?BxO9d9rJAM`d{|j%KDslHqKY zO4KW!ulR0gMt(~f8g!`7>Rg#nhBf$e1`uxocZ|$&zJs!7xP@g5S>wwgBYHbRI40~c z{zhwY)KPYQoT?~8$G0`GW7C$|`ZielnWkEz5Tcps#u3hz>s_EUuLB&P#4xE} zP3#7Zc=Riyk%W2RAr6$EzQou!tFF%lgUn~hO#!~-LAU8CNa;VfdEiB$ciQHZs`!YLHhvTNZ*GZLZ0Z8 z0lim)p8lVylyH7dQgpJgLsF)AL?Pnf-)b8(49;V~fBb%kDIdhOz(#Krfap)^wpy0> zv)^0qNI%qM7@_tR^458FAs|-0-ESddlNEDtLZ#L5o9d;Is4~6^OIOWy1Z-Froh`@P zxnAjJW&M;UqEi<~O5S&cXhpRzfvR##M_tBS8ygLj^cM1YuT80=hs9PD&08A}2(B|$ zj8OE~X4*c6?Sni(=m#MV1ymOvj}OXWoE4tyw)L6BO68F<9i9I9NZW-Tdb`6TZGgjS zc2uGP35B-onS+^n$_^iz(1r;~@BiT}5;aRi%MXW`+0u|CaVe5C+{kx&RL7-mx45Uy zVL9q%xh9Z$NT-Z1H$3c<*d@pCeU^w5m9?V+rZV_@$2zCStDf1E+-TQU&&EN4ibxO> zq^d;-K->6a=&1e`1xGr^zb@c6nf@>b@t$1?4#;!p5$9c>Wch~-vS-4`d?3!F#fr?0 zB5Kup2@N$S{9(P0$!*4ZEXrI#JYFKgHq}y2f5jMm2mfkgQzJ^(9uuw^En7Ldfkmkt z5?Z;5VTwXN|NN;3yuhLH0qV6)rWk;{6clci#U2zL6`G$`Dy{{rYbZxnEFrXbYisz(mn1?*U@`Dd&9TLR2~eKs?>wN0)leQ`N--e6|qV_6=2V zL-m2fYkXe_3RM?mpA8tD8aS;2%n?bK=)=?T#6w4$f?u&}Qwqdm0nZi%rMyj#0B}(|s zpqjjGRFCO`r%EIWvl0Ge;!j|e-#0lw?3km04xh)(7l-r(H}+=5tz%S#9HkWI%qx5W zkmiD4VVwEEu(mAs-4?er4d}}bV81;-+@;~;#{Bxg8J--#qXHe%Y4T8O6~$18AhL8N z-w%RJ3vro{Dz)!qb#L+8CtIoZDok5u+u=p;x z$EfA@VR#D_C?>E6bKu8HT&>n?w+{{j8i{=&b#ZV4@+G|cg*Mo)>nqmVlM<=v00Bky zL$6MW0H@a%Os>rDVWz|;eo#Hzh`3P+9?v!qjlG@Sr7Z(^V^#nOQq5q7`?rxhviAMO zyF{T=#a{A)yVl8+4#ZgzXt}jZT28(&h5pZ=t${G^UVjhQW%E>+&Qo~|ZwjlDziV#y zs@)AvOruzzc*CIB*jqAVp9})<*w`RfAbsfG8`nN!Mt)4#0-*Df1&}2XdB*Y zxHndu9LL#}_slZhF(1!&j+W1k=)Hq~Ij62FJ32veE_P!!Pduw#lL@&u3kHK#N%Yam zu4=^6TXOOEp#T5`vCBm+vEoZxDBeVTe(@0J_sd`lLp)9tr2#20UgvCB_ zgC^Kh(D^ll?b+F6d_1^14bi0(fGGa0Kj-fv>c1P{Qvi*coktMGgs!-s@T=MA1T`s$ zJ8ll#?~W_-b-5v4{?_E~|AuvMW8qCKzon#vaF3o|Y7}#8=-vIcQZ>B`tkbm}qVpQw z*Nrd7hSN5E;5LOx4w_RQuOTXd3MbOl4AkDwn;!?Xzq)(TK@%Y^=$395*0zXRv^)ad z#>K$)Y!vZV0W*o`X_&P|_8chTo(N&qtrpfXo+^|w7Jer|TRd)tx4c4Q5=IW$Af{1@ z4ufubTv?dKuUMu)_%K8L0%cyK3ED5tcH7w8Xjg++1O#La+PD;x-|kw-CjYkCMBv`) zV@Q57KAC^7Kbf4gpgu4Aj{s-=_7nM#Zz{sr0`Fdqg;e!H>n$I2s!T+%n;PiKrU9!; z_KME3N#@O7YI%W6N~p^1n@H?K4fh9D6?JVAH~Lqv5s{a!X{TKz49O@ZY~cbfDtHWf zGvn0?bSKJYuE3g@c-vT-75pj&g}FeI)`N9_smiH}*=k0}o1clXf8zC(az94z* zuiDsN;IqROV{#gWM$Dxa0MYPF<4wUi&Ai^&JZ2}H8>XA6&=#*-_DT+IE%{CJq0#70 z#a(*CZ8Ibv<>DmM#-ZIg^vZ4B*yrwYF3JEgM}0jMRD?3FgBy3S@!kw}p*r~q?-Med zlxyz(CI{L8HiyC{$;Nscw&xo{I}Xp(4phD6WZt}^g+xJtZf*&;tJ7LxCgppy6+UlF zJ2+U8OGLrzj4*tt*D10(N(3JyKNs034K_+e(1mSJc@yzdoJ8GR7$8Ze;_1ImrwhHAadlXKy(r%5L zMM+w%LA&Gn;${n0{AUIJ^RN3lCW1NXucSde$1S!g*df9<8ux_|3$MHrCg)TB*NmyF zBHCh|BsrLX)lUe`|7(TMny`4MIM@zH?J~9wdMnLvs++116CoD?pMEMgs`t+cXSy#O z^C)>Rq4{0c9!nQl&DrhooQo7?v>2q@+BBgCMx1qVL7F=`v;GNe*RwS8^@%@kPMG26 z1B%;z)92?(C{j`MR#ZO{;aR^|kba94=@3QsHZk+Fw&HdQRiNmiD#D&}Qc@ll5wg9n zCq$n>nH!UwtFciGdcM1yJ3I%XOCGod{)IG#mX)GT%g#qe;jc~wVEk8f1(xQ|LHY(fw z7}m{_C(3;7>~Nk!#wh{fmUN6Fq%9JGbb4|P6kW7pJ?KS^n}5^32n^|PSR^nY*8z%d zN7%bA9bt5m1szHcazk4hu1lR#QExuN@nM3|03-KHjG_{kUj-1g^H_|VM7J&Y#*9@m zj#GZnvY?2i&Myi}*gM+_56`2kGnH`EJ^tP!?hA6K>0~Aqgo0OkVrwcQBwPHzIP_br zJmE`UiU+6AE3A@v$VrBKf6$;zhRs|A)@r4Shr9ONE2%-9th>pTaImKQnW?T!Sa1%@?jpH_UMB73wtbwI|J+iv?&a&|<&|E>H8u985L@(6I=(?Tvr`632x zrZyC;*ASR61TTN7*gH6|$Y2thVLXT^=iL+D=o0$#h_#dgYKr+m54D zF4IAsOjwbv`UiE>sWrNV*<(tD2K%Q=J3}L99`I#4x}s7}(I?L&qRQxiE>A+%ou9(# zFR%;o!CC*q&z+2RuA=3UUj)5Y)6RbUNnJjLPx$)l!Ln$vXgr?0wxf8gNkmpSM^u+X z)-4V+b>q*&LG&nmWH)zZW|zh%)-OL>Vm+(2U-tTq|F;((*mBBBllE%G1eo7x)7y0V zYP9w?1sh#Skp`ZHUXCGxxNiiey9!%U5{>>&oT5eVIuUBYcVQGwzR(aHwM zXeJ_;Kiyo4j-kC^7Um20YzGR6Xo~;2&UeyNKlXCP*}l*7G{#?pU(jmZ@lX4Pl^TjB zzo*w9=n@51=J+_(;ut*AoJbUOzSD6Je*l8i5JP3^&Bw-&Q}GU29$R~qcxq4Cq!%sy ziEDd_M-%w#CifFjE8%|Fnex*j$nbUDVG)cEC+P|!Q%gNG-vl}8w*9_jE&|zQX-1nP z@tIfDsv2gG?*gkDJgTjRsyN8`Q|zT+Jyt1xn>db8` zsIolZG}%?MC@kBd*|8qt^-YqW>9>@5ViW&u&mI`?mLKLHR39AS7*XOV)0p_j!XRFk z;L>oSjnsZFVlILH?zZL9Qz4Q73FgaJ*Io0j%>0--n17@#>MhA-LIK}YX*iKq8t*zs zTq8;`y`KT1moG`D0y;2}(4e!x9?RmN_!^g?jT>cBJh{E)sw{@oi3{17MWHi#`d$U7 z=&j@BelN7KCX9n$#VOOBYtC0DXT>|}$8C)8_XWjs=f@nbgpiPvqOVfQ#W}ayriSac z_W(ckMM)p*WIGq5e-GX57{Xr6W*X_0Q)(lmI`o^4_(s0%V2GuH2~VrGQP+Y1&bbyk z+SZrXR5qA*1xn?bo3Dr^`Dd>a+g1@E+u3!1^aISQiU=UNqW3d4?Nk0Tbj7 zt`7($xpjp0$;uMepjIG?R;5(mF9=#2~96Egd%3KXw2UJ5gkTi(&JGZbL?*hoD z?+0;lq2Q#r>tWoBd178kKK@6nc1S*8qeA7^Z;K(VsgdoEq|0daFE|sUiMPgh;0PxX zidBpyDX`+anmzQh!PgGUBAc|jlzx~}ez}HDSzB|unqO-AJ`ND!1R8B z4%)kf@*iy<-p=}-#;qKGSYWZA^pQ#*G~$Kvrd*%S{7=smJ2ip#X5b-L=ke2in?yl$ zdX=-Obah#OdXFrcY`wZ&dTJD7q-LYUm|dy z@UBr33~t32b!IaVDcy4*4o1Z=MtlN>m@W})v0u+ukw!@89WqamEQLGr^j#&#QS=dFQP$O#aa#FF!Um$)v!GQdW1l;36 zrJ9J}PQR4~BhEX?adiF>NB#{Wz`A57g@&y_Y|uO~l-$ElBGOu-J|B&rUPVaa6#`u0Kj7icl^R~xK@jVRE&mG?9Bgbn?`a zoler2mP?`{=J`uI)B-Tf0b*l)atH5`WIoG8;a{)P$0g$TKnxehp~Cbg^%$R{8cq;X zk@aq+w+6R~eR)8|Y3IbDvvl-DDIWzo(ze(|Z_H)3bEQ}V$=i%ZUB37g`ib@-Q%_`B zE|9|!km;oopb+N9^e_@5Yn4OPrM~7DeV{Pzp4b&O^1xV1yu`ws_Auu*cDt$h^Q0`l z-c{bih)ELX4}0QC#HL{vTz-`A6Wto;tE>i(h9peSDW&bGUuWPXjdG75!7DLRoFF51-4^t-Ymq3cB{ z__#WqlYJF)?UcF$=74m&(LyKrF1cB6#l#S$r6Zv+MVnN4;VyiQj;fSbXF~$@T*wUi zieh@~AnZ&T(k0Fkzj{c~mLquWOHFBV%CVKfLD`Y3>_AdQ!ad09nK6u32=}_BfMgkA zrGdw*@AHd@O*ay>mCPO0HfFgQgnKQj#`Tf)PCC3sr|1^t84=6M0F`n%BJK2kRCSUA zg9;7;y>IUYd!a-tub^o(3o(h`V<4>Rlta3l>9f120f~`7Ijbqsb$}hZ^#+Hpjvbg5 zxa~(mQhMV$%993&R?`JKyoCxAW zUrSO`M=KH?!n-*`_|7NDN2qg+Da?i01W-8LC#eKT5iZDA%K5~0zJrc>r~J16(}3;Z zXc0aAXc&2%UI37#PNO(7XL{>;Wmi%L-Gx};-j%OOfSN$T(=CdgymDvJAw*>nU0+*} ztXYm1ffR+X;x31>Yq3ab-5*|TiVA2{ln00EGk1@FsW*tSr90SWXbjDGtb_Tkco_NFx|hEZl1F{o<8mpyBa{|lw{G)O%D6#d>zmFu=f9fJdYZ?&R7cLX%RM`oN>5-M);FS?FD)wltkM1$S zl0Nf;bwRApBmMob(|j+jw*i=2kC`Fm#$D1Ew1$eb8u?*iZ8 zhzpmd;-UXT;t-Ys6n9wq`M08xw}HVzpvzUa+NLf2#{?e}qsM)QOG#Y?P$SqTbPi!v z#Z{n5M3h9f?^c(Bu@;a$21315w72YjZS+2vNiNW8fxmiD{B?Id0bMDNJTwP3K?Onh z>R%y7$hqE}hMRq3Bcz?{kMx#zNy;_Q=JVw+ccEF!v1y3M?o&9L5?!+m_NABbD zd0}A=MB+z^gCiv@BU-EvE7jYIeY7)sW3CiZ-bYUX^?bMTQApjW*_zDN1QBlp=h7KjH#icz9Dmd_b zyDA&G_+IvP?}1R5Qp3?q?kIt~3npfmoum`r^_1)&6Tn*Il(AgzWMk~77?PQjBUYXc z;FOB5w4zi1Zjv25RAY0jqvI9~lh zPmU$0$szY1Og(0mfMZJ>NVtaa&H<@$b{i0cdQa5g%P3)tkM<}aJ5k?O?2{2ILq^v6 zevqNBg=`;;;AyjhEEfBkoF7K){=W^-)QS_yc53Hb7(1iQ`id z4Hyc3e-?tVR^8+808Nn@`eu+17y_r9_j|>z?c*~qkD0f|OyprF_g~g{IL@TF-}#oS zjpDVgsZGo}-Qbv{isz~dqYlR*T!ghhRfLo(JUF0Jt_eZ~1+*s&WwcraKB&CgB0S1G zASdkS+Qfbu(ml@qo1sFlMOtldHol#Ed1p&{kXJ5aj;6Fb=B3;D!aF8L4c)U}R{G?VxN7qP`bl>uB{$xRZGRxWm!N}oS2 zoj|qFu4HBMpw&&uDQnV?vFCxwWD19BB@53CixS;xWn%*Id>&LJT5pe4AQ+R+oL|t> z$~CekcI<)0EP4bVG^6KSeQ-An%YJuR=B#lg`nO!BP-5uL{yYpxxR|{znlSS1tH1#9 zCGHBLi$>5ju{WQ2YF^6|Hq{guH!iSjjftH@Tox6Q4xs?oP*WR8sFwe!dzgPE6^yy5 zD0@r}cEb+JGYvxCINc2Bb3ED3)V}RcrOkdNgF?6$0qL?3#TyDf+5=Nv^}d0_QD8Rs zcZ?r>%f)a6?IvMt84mEKfL0)L&C9371e(~W00P|13-_hJyjahFu!JR%5DuB%!m@NI z>*dT39s1YRZoe8Rzl;f(4WEv41mA}~$ITWp`Lhte!HmXYmBkKv-Z;yVp6q^&cO*C# z^Raw4V=`0Hde(~H6*s13oK-QX~k5R0_8FOqz0d89}pWRC>B;GUATGYwa2wJ}n)y-7eu@ z#ccK@wjkOnVF*W3a7DCZ5!#ziEySNSHE4qWjHET^) zNq?Mp#{FKE=I7(2497?SJj!J{$mWK5+dm>RO_t{V5Jh4}MvRT*!twGl-}@>kb*L2QuFbWIxx#7#1bDDD-1E*94^E{jAw>utu~a5 z648+r8=Jg?ZA|Ui@*!v6sZzzLYPkmBRERpNx zp5Tr{p7F=!Ti(bG#6$*_rpX=L$f32hegVXAVXPE%ziqWy^+vh*-JWj3b&gGzs#XiK zQT34Hj(Gm>FkKav^s$M%;KRm$@cvZnGcF?#ax2xo}#ex{+2}4 z3|HXs3mIWE+ungHp=|bAsrl6N7j-hGa#2A*9fJdgpbzXiqgOBUC{p2uC}@zO0XTeh z)-4q*MQmmCuX@q{!_ZE_(^JI`H52TPk>&J!w^^r+H$)aBOE&fa7_-|jW9z%m&12Px ztLQlPiA8vW5H0osf|)##vFJqS=Pi#`04-|~*W3Zw4%X5;|HnXqZ)2|Ziw3=q+EXjv>w$@ z#;hdR>^*8(dz}Q^dE=co9p?&`^@v)R-2f~H3-^}=29Y7#*CWW5T8U|-MO zGlFdZm8OR`rBABuIzdjtMVvQ%Z-cZA$&N**CBvr-s z9((0Iry`qYM9**p^Pk0=V7TCxT0pvCT|sxT=FX;Hc6%p-Gb&z!ZsClb|#3@WK)2$=;;qI4i7l7&q{G0S3}(=5$ifo&fz$+TVsl=4Z6! zQiF;LS+S4o0xC)&(R7qPB_^`+VWTAAY=YEJcjvIyXxV0RdqQu(*mOK z6q2F8@++)hmAjiX(9S_A0DZ5+u3;#`L=4OJSvk}s?xQ(hV6QGTMZ>lU1R;1|y*4&I zE$wi-{_8UNC3Pxook4|l%tA75p6}s*)gk0EBA%Jz#(o=0bI#~ho$nGA)w)f@0aKpA zW~{JZeBfb4K|*1Mj6I6d->HqJX06$iw;SS{?6u4pKb|tupDq2J8J}Zlnq5nZmX~{6 zJtG4-016A8Y_QFHUps7~L;p@i6N5XVm2*YW+l8m}Z#aGMVKV^Ny>OF%N~Fb%PPR>b z+!!*I7Zt~^i!?Q7*KO&HJ&^{L_27BW0SYm{^q}2qnOw~6&VB#IZ6yQA+(FP?lgqhW za9W&TeH4$%eY7p{wfK&896hq4F_%CeY_aBDf^!HE?M{=;vVw!o{q;7R7cQupg%i}KQEA}IPi+G-9O#=(0sY9aPf12c|M7_n09Y}Hnj z$ZZ8jdx<|gk(Z5ckn|KY*-|^T8mz8EZ?3JJO^VcAZt^?3*Xx;yRVt%R9HK(;3WrXbnyVhwEUOJM-bFdv#YkhstjLx96(f$z<2G25{1XCqgJUnMvzK{ z;b=4kOvUp+_$7Wax1t~jZ3|p8x}_h(uAzdBXd=i$8CzC!v^M3W+_Mec47CD6kfE*z zwGl{i9BY)W_TiwtzL&SXxX6D}?ARLfFU1m{UCWFyjPG*ekCb&F1(%PN&1KB9i$Oo7 z>QzHcAF3EKDwz*!S5A#yk_N)iC>FepY|mkQ`e!DuqNkX$97&1K&E>nxqOKDN3N$TiHE=a-a&FTu`zA_asVAUv)^2d6TQ?xO88L(d&Cn&K`<$&IOLeKqp zT1KqDw+L%echpR&!7NlhlU>b(rj~AT`)17iHkredTI(p2Vr2Pb78=%lXPOktC76_f z<+p1Eia~)?w-b8;pYewhADHVO$CV$JZ+*X*x(Zf}1ifZG{z&wr^ibkh_lCB@#1bKA zRFrh-^`-_OzaPS9 z@+BiuqO&||W7d89x`^ucDqJ@?wC1uKve1q%m0-htTNa*xi&tPyrl@};T_RA(W`jz< z*k-zeEocD_ix7lXJDFX-o*9Pz+)NQD<(3Ts8(0CM(zb|u{sJW)iihkE^_qvILOC4F z2c)R;9{V1X2(~gD0t9L@o>Vr&PxNTleFLZ9a|pdrjM-vu0e1o;h_KI%4CjqUdd(5s zKI~%z{ITiy*8+W(@NXrp4LK#^devY&2 z4r9sXyfmeF&`za6Dxku}bz)-_f1Q7^L9NL*GUbXu6t*Q$QL7^fBSpV)JV|7twp|!3 zAcd&(RhTmtq&qIs@-Gl)iT)Nxl6Ck3U&{%-_-7S7vX~(fVr2bzb3d&pnD|mQ-F5Ru z_->!3=hgV@UA0vOJ;@J(L%;eTyslx>5th<83Kfvkv^bZbROH5u7Ua>~<@WH&2dRg& zxNn^3$u*4+U;k3{rlh^z?6%ddbb3apY^v9zBH<6E>NTPL+-F5S#)*DS!=7`Nt}yKU zO2yw5HYAdjSXj%UJU{>=uZXfdJ!8~7`{TJy!}X@DMYzsvQ{Q}Z?)`hi-y~o9wS%gV^5WpWUnvok~pbvRJAmZzK)uL2;jb zx>FFoj&Cnt52YtvHI5pQx})Wc3imAL%Gb6&A1{7(xm~CO5)-&_6|)H;KL(HuQOZN1 zj*lrheDpY#U@6<5^r6{gRz~lh{#U3a@J#>1Wn-J=X>?&sky7p zx;v96ut)g!0N94QLP(_p(%(*+)u>yZxGKx&K4xw=n;tFXm;9xAv)7IH0 z&}TgMT(pPHPgx#&3JRGcbzkdikvm)1yz)!S=f}JW=Hl%Q2jWiKSHajC8VB#JK^vB@ zL9t7d&f2}0W%OM8_~ybw1jj60f@VjSzCXuga=T1uqc+xw#+v8|)C1m=i^utin z(8N{2O5rIZs`*a^)cE0{9EMnVvX(ULSOwKPpvuyrgD9nXSsvhYFQLETp43tS`8pP9 zAL3HHon=b3R?A6D=2aUunr(l@VT-bXB%qK^JY5$0ITQ5(pvwItto5ur#{xmxVGl5C zth)=tS#(Qp?a0l~Jw4`lMWc88&7gktXvc-DV2N}fK3sGeC}eS(_uf9xlDkucmJpyP zRgOrdW=m~cyZBJ>1;3q8(to5PhnQ{S8xWh<>b8wVn>8slpaU?xU<}b5G<%q+rM2HB4sC3yVU49b0`P0cZB$5Qj`aclL@fg z>#=?St#9yc`>tdD5iNNq(xKF8sPVx$vXWmiq2$~>1d|!2nxmbFKH`5?h#Q=@1X%x0 z-xD=exfUmr>nkl@k-7Egv@>S-ua!=4L5`5p~gSe2k{wdQjmpj5RH2|VqUtY<;DD@JTOKRzX56Y~=fH~E%KU!Phl*av9 zIBZryA)2BhHvL%;VkE7A?f15^pvbv<*zr{Dz_n>>X`vdDJ~@4wV_QjZno%5zy*X>y zam*B%V@A0Zr&{|PvWykgV?6My4i1ivi%t8jRF51$fYcpeDv;0U?MW)A5dkdA$;Y0v z!I>wC7*1%P;@Bb){8ga7VnD$ZvJ3(`vms}ha4!6Jc_T%35BC8xGVTJ5s-bIyCnYpr z2!GB9?pA`1))RHP#W}N+C@)gkhkFsKQU|~qv=KwDBs9)I5fRhrixM@q55w|33l0QH z_9Hc<()<<)RUJVp8vTyZ^>uIbov^>9I%}lTk_Ez^`~SMptXM~veozUj6R8ZrB|iDk zCA!IVxsIc8zB=X(ipfz{Y%Y4Zzu#*|qq1YD5xg_|Ej)d$KjdCC^!)Z`*PD zFZg+U)|yO#T>ski04&?)nJlT?^X1%JO9e)5hq6mi#~^ahs30QWV42mEnvtlORjXhNOUvwq@_DL|-F zK%Tt`#`f+CvQNia)LhgTaJ0$hiV_1PAf`jl9YXA*;w=h0IT_G(DlzVz)?EVqX%cKN zJfLL_6}PARxAEPhsaiC&?z}F9OFP20T%~a~jYq<#b`$>)0Oondr4sMTxnZ4OxY$U20V zW-$gYS;`bFUsPBa{B7gRHQtj44GZ$vN7fqx7Z7qELr4R^B3yhf7|Lh(r(b?O5hKV) z`hzevTM2zReI1OT<(V_;qwBO<8N4A;RlKKNWwKYLpSZOwf$QME7bpW1@bN*|f68ac z=y+KyoR~H>Q)9wcnlod#J>0z%Unq{L!jb)_A@4$>PBxv}I-#{9RIbnW)(!_5AFvTI z##m-TGh(HE^Eju5`szr!pN-1pDc-ix1lk~=9&ijT zerm708`d81mbCSKUvCw`f&*5CK`qHH^2n1ok!@4UNPIMV143lOi!xIh>0W?MfJB*} z-zxT?{pznCALeL{jY4gBhf_r%BIF9|_6|ENpCBOa7g?8^ zKtzO=juErdw`&w7Ny+`Amy>&dUDj01v$xai$W}@!I#H&wH2t=D!OUKA0IpE?@>St5 zTf-bHaG0XPds3Otr;d}`Jn>#y*k=97x1@mmqo8nhxW2zpo*?&2z13%Fm@LiAPJI)1 z7!#kTaSV>Cm!~$XRkH2IamHtU(GLsfKqEin8-Tu?0000EEHt`L3e zeO4K8tzl2SV;kx5qdX5>{dWLA{m2f0HZ%YEI14hoVeW^2@2ZYwNoqowVfu^D3}y;J`VFfO;fYSe0U<53xRy?0{H}{Z~ zukba9Q>cM!u8^Y$Od6Hr>BQpbS@GyX-L?O?bC9lrk zVJ`F}^DCJEQsVyND5tBWG2!?E6-Ky_x+l%jO_Nw1Ulco@vlZF!z94iRJU2S7>3@UH zpKPn?s5$rrY8u5Ts$9%ui0yb{eR**da|i$`r@-kc#1HFsFp0KL^l#N912lLHu!#t; zfG<84AP{?*Plb7aA1oU%F-m{heq)kVu0?xKX(klY2*WLAR`X%ixp-+!yl-k4$j)YZ z7r+Gr%>~48^yoUT023YQ=N}!EOpci1%*hhV1q55I+J=S$nB&OyR1gn3bknB3O{!jE zIL33;V(Cdg!BTlrN!b*2W)Ko4rWI&H>d%KEF70>?DKcEagUp`pY5wcku?4QgV#`Pl z9**aJNBL?6mSnjZP=V?`;?j7I3a*sZ09ON=G_Eg(KCoGl7|v`<$Epq>I}pScw}rCH zK4~sreUq}l&H~?bypPLczHcXG>f}_|rn~Q-v7AU2QCNg1?#AfEMP(;#Lrf zO<_*=&C+9gTUc{24oQ}hi+Q;j6&cz-!|<<1cJ(9ZzDz<}QE&F(Yt`8IpxDjr=q`u# zxoCED3-oxWtRhVw32awBV2{L_8K=684je6d^wd7RI>(wV` zcC9oOe_Pd3SGforxmEz6nBK>3+*aKIJFaw9MxyAK6>uN@{B(9|PcB|QmU_lOHTN@x z8owHFDpoMOpSV*)H=JjpH_z5h-zv-So)2ufzvv`2xhdEu?jJKb!<&oKbi1BcDk1E(UAWL+!~doip83q}x;6 zM+#hYB+pO$5Z1td#QnpF%4~V9n9*I<&aAedx|Hn5+B%lt_-Z_9068EMrbhk9AP@qn zEVV>9RlZ#d5V$MbGH4K`!I6keH?`En zYaK2?>PFlXuFg-fNU#hVm7^9KAKeEtER#W{!j*6k7tW($6lPxCl+_e6Q@VYG@{oz+ zFA_d_cwA0Hy=mKk(#+RX*0qsHupolIDWEeyJxHH>PQzv4vQ*PD)r)C|>|enhIp3cc z2)4V}vDNvQHRA@hu1**Lf7eb{>z%$D>%kasNVZgA;w`QEC!l6Tu1n~UH;y=0tEnL1 z`+>tpkr&k%h;#|0%0ZxRZEuR3f7n?yEUM=B(BX=awox-$%3^r;YKrpC+*AP+4S#*8phFOKT2P{q$Fi`Haimj#!Y+vlj8hnUfEC+EKO^Uy z&bZw8&s6+6Ap5J=K}&?|8cE#qD?SQ`8k-nO-xPqq9-S1keRDRd=hPv6Bkj|o0-)MX zvjQrPxtKh8ExwsNmJ~OR%ZxE3o>8Yde3vJ;$Fo|eiBvt&t+6#_A5q%&ib~dfJ+g|G zf7b&@@m-I7j~Ks8&4T|CW`>}6yZ7wF+zo}9U>#1mJZxocK>2i3Ec#xGbYYNHmOp?9 zUugEsle~&;v>FCtn`jn=)8W|4*Vn>;voh#{5|=ylbubv~4kXlcK?~D0@Sxjys=1t0 zmR+$w;{x29F8XTbuwz+!e&Xrw`917?v|J*3GpVjZRnS_X#o{4cDgdj+euSq29C#}v zJ~r08-(0=%y~)cB@hhZ`kMFV=DhUA!kQhcmup@M%?<p3CF@Q(9b6mD=g1}uIc4+vh!=y! zE_UavM}y5z=NXiX*=rDrRdP|ibRZk`S zozTxAmv+K3`L|N9tsq2eird$H-HTLYjMi0Y5bLn5F=qQPGa66pb`rG4JIflAomMmX zm))PmQG``orPeumP6e5XcXOOIc`(CzpXG#qLxH~4=m_F#8fzWcAj{U#nCWLwag;jw zY9uR?zaZF>or$bh5F=YpUAL|&v^z*mY`_jm2enOOUcr)B1g3-7#ZyGbbq06S*HP#n zjER82`4^d+MZ^ml*OQ?>V{-W+pQR9vN^~@c>Pl~_*}xm64E3u^TM14M*OYDj!?NEp z-zatv-*T`McY5zVAo0i_jjwk3uL)r?dHC~|9hgG`cE+oH(q**ivZ zqybh6*cOuYC3WKdloU}Y@I^AOMct8ZtC#LSsQ|lIfLe))u-X%lBI8pY@FKyFEt4v8 z`JdmzNZ@}{c^CAteEKcj1{?%yMuZKfqDQMyWH<~of9q!QdclWqW0ZG%Yo#R2H#@%v z=}itF-`xsK^(wdZRz*J1`j0OAaY~%D6vV7ZTREtgjIHi^cvRlzk|ZxEu6OMe*5q3P zWRo^S8WX{qCJn1QgxrB_ouL6HDOf5MRJ8$lp(f3y?400v=pOCxJ+93*iY*?*w3*&#&^sM*BpVIYP&1 zKevOm9OR3iS`i=+wb*a7?~F5!y|d8B@xYtuM*%jue^#Xd9XG1AloA|A>%d|t3~qk1 zq){J0{wvX7K+KTq;N<5s7`-T_!GKRepsxQ$petxWbJ24st2iP#B&Djl?(SGMr5h7C zIRa+*lOaS>0+>u166YMpl!hYn?9VyKT3s-Jlm)T!@fR@V^`OzMo1Q=#SIV@Oe*0P? zj{6o6fGz8W`$ziwSPJF7)oap+!qOp+F@r2gwtvB%^+MZpVADD%=bt6G*~~JnoaWW2 zltQ_+&imbp;T1Jk2ENXlMxjf?4F~S|u)9Pdo7#O2zO)u(i(Qx;EPdP7hUsl`G=*I& zfb+5%+3%T@^h)lQdSa$81K$O17<&qG{D1WNK1c~8TZ$6Mx9NfLRp)DI$v3YZI$G{u zTuRU+Pa~F%lUj>iBBUqT-CUsnyn7G}obG-?=-k%Qe%0*a^JeNiKe%N0_>6QnF-m0v zC3vWRIBlec==?sumx1zn1^(wOgm-(F&NcV7Ap!9HfWfR^97f0mclJ_c$n7R*@Ej1j z*nhZxCpmP5Pg>CtvVFQgU&MpYaws{=#qA!S&&TaFk?r#3M7QPLK;|U#0zX>&RTHAz zuo_=p4xh;^QUg;dE4V zdmDWE;V+`UbXg>7xE8RQzMwrGFvky3!B$lKhLq$r{DRftUdj@UgdJk^qU`ddO5O4x z`!Z)o0puv(HC%8SaMr&AEAC7P+z{tQ`E?egvF$Yr)tTRqy-P6jKseTt0O>pa+<;FR z(@h_>OCHJ;Tdu!Gr_e?Sa8sLeV^JQW%7~NAD~=!Epf>5{lpx&9+kV^NA{yCT%{IE? z&st@2s)VCSfe7bm=tP94;>fBrwj+MF40Wk(T#W@*}pUw>fNpkJSWA%fIK zl2FMmoI%xfgl197-C$mD%v7r_i7&CcRN)87_ay$XJcL<7Q87(fzTH4m{!G<^B5aHz z@`u5{>~N8c68{wi?b7Hscb)iBIj-7&r^*C`6TRJ<%xUGST%{LD$fRRQ5*Ca1eM zMf0Ex-}um^+-%6xmhGyGm6IP&8m_OWGmtb(+wH-NcOMi@by!jLjr1kU9Su9yjGyny za5E1CuL8ijX0gZ*YFro&i73lWEF3pnqMc}2#}#w8rL@iDPtl14$V`6zqizW2f+-nb z_?ZLc7P|LU^Rs;nFld(|vH138>;lkoY~JwzzY|=s3Z;0q<)~{b&}5~A1_)AJtnFu{ zEP&^y6KN&_$E1kXq0Jv+)b#cpIKyU!now6Swy;~=oq%Hh2~bOcI#@-S32;63Kx2SzB}HCvPnF3rtDDr)z(;)hdZx~VD% z(HJ|1Dh)XhfLd~}*CRX_=2Rp0vL*=DxQR^vEW@Bxvj#ov^Dl39-S zP?(t1?&EOsyZ{)EW&^2%*ShF0fbAG+>&7gYOqBtYuzlcyd1)>_heoR6AX|)80DBKW z(?_(`soPu=ghjTe<)t-Hvg$tz(V-Xc-%iF!$0?hnzU;!3HXA4@9Eqz3-}vi|8Z`-j zb+!kn!M00jm|~N-M{c{S&SC1#(Ch8fFdoB=^_OfHE@7R_k@8LdY{yIvDUDt5N~_lX z-v~(oJpw?jr-@TT1Yrr{Vdglr0>gV00o5vYVqaq8NbpRe3`DHxdY5?uOW%CC7KABc zLbp3fSnzz*r{*^cT8;88?lzve7|*-I8)(>E-8U{ElQCf^So`huX6ATw8bjy5cvsgX zVEAOG@giQrySmA0a>hsr)E?7>5_c&TUJM&e)CtG846fjWQJY&qY!86hZif5zmo4y$ z0^cBEP&jx{t$Gobt(Q-o=Lrx#vE>ij4IeOU)s_!yT}LQd#&LZ=V8k(b_@(x=kIf4x zT~xL#Vv9zKBu|P@SBzn*;B2OIT>)cUtF3$|gu>L#SO4L1I#|gOJP46Xf={-!X!q@j z>WD?Q@drx4_#}apMfI*u7?*$1DubM~~>$J1jfBlho>oV6G&|yY(n36{&|1`syZmY2|z9_>qCSIQ}PL z{p*Zt`YNM6&ojeLEE%uAyyR^!B}O=#{0$9iSeCVY2|Fs6GV&~nKloq=f* z{6D-L+zv(Dvu!1=YrM}v%p}~fzT<9L!o-Sge+QCc!z=wsUaRDCJ#W3`x|I?vPWKkx z&g-Jio*gd6+Um#10o2@S&zk3lU+O_3x4%zOg@GL4-f>_ik){_62r-yoUmAfXwU(!D zh1=C7^0BG-xRHf7)~%)dlJ%dyN#89W11emw2H!XI5k@w?)nI4dq9%bNhAC_+O*-v8 zs^Ah!;yV_*Vi|TeY?0aXM_C0GjF1PQmVhC5EGOsYM}7`*QtU8q|7~ECWMvv}`dm z)nO**y%8n|RAAl*MDm1*!HQybMvg?L&)|X+j8azVr-)@_0PB>!&4q4FpoeJ2%_>?b^~E@<#!hdCghTY?I>n;x+s=9MnVLBk^X{*O;2y)P{n*bT@|mzU;l#FIBb1zz$WeZW z5Z;Bz)M2OLq}p7mEs1*AIRMCw1$N#}?NOt)4K?k(!U8u4sEezr8)5a$g0qXXT(80E z!soNdV*J8Nw&1UR?Ft60&j8V{(&PGTS-6xv`i~|*1Nlp+5NP1i#KWq7jah;*`M$b% z#X3k1Ji7zJ>~P~9h%JZYdDhbMv0(Onjfe|(x!#PkP)0UqawO8u%WhGdw}bgrPMGG} z&J2UyD~+*<^rE|iB|Y?&TGf~01442UFh!lb?}53W{)c9@o!3WbN#7b!g6hBg>^{{`Ep;?=khr(56!R>OdH%FW8 zDt-uK|1}`%N)FVj!$z{ANOE!_c`YV7pgIFh`q0w71yqObL3?ei3(Wig!Np{Px22P= zebj|jgLBCs^7zcrIf{Hj1dX=OzD{3emcYo#UG zJ@P8%H(c#KLZ?ZdQ=7jU7lFz$#Uocf6gD2^i z?t_$xL@IK?)vsU*c>r~glt}j>nf7ra>b8$&&f&v$^(;aGxp&yANh6KuxiA$Qdy484 zcDda|v8yCN3uZlX`&;W+_!v<>3m7VJ(p3;MPjn4O^W-jCy)sLwo+AfY)5}wQd)43h zfeD~WhhXfoV5dwpyQUN&)5qu4Ie~U=hE+&F+*(xB8+HpOiYAgS#-X6RcbkkCsE12$ zP@D5Y0JlxK#LF^`i36mZ3P!4dbH4qRbME5~O^c(2HJ0RFDRMliiRIz;%hbSi;2p2J zu|_H=00z`Fo#?o@WEBbqRf8_g#u2$;egPO(+OHPy!2;>SXjocDw?Kqw-W*#PA(AEt zi|@aaZHJrjjk(i$s+L4V$yHy!xOj*BaZ>DC@qc^I=tVzcU0VH7P<6`1MnstSR1&br zVB5F%KGlb$E8@9cqMnJXJA3r?BIy>Nbp^ryW~C;XxwJ&=4~K1D{ZRnO*zBX=fiKZI zy$wTX0LA+xVu#=6vOTLr^IY_)?w7n;fyUYxaSmOf>bl`w==h=o~MGK+7V`1b*E;=?;fxc z0Sk@la9T+>Pf*3iZI)3+pyg6EV=3$IV>{2gugkG!7+aW$i)rzxXM>We z!C{arSo~nY@7EtffU6$#o|Ftdascv&z6jY0_uk3QBKqH5-tb9q?sMSdnS_lYH#@Q$ zfA^)~1JCIz=N_;Y_9|}Erbj*qf!umeEe7&Vd`oC>ux7?W9HLg5T`=ev9keY}M5HS{+ZxwOI{O=-*xIQ@_uokfWze)wXp zC?405WVW|wsWfr{ib$^kG&)dlZphMO^wbgcm}t2rPv%=MdCe}a0Ri^+aAVnt66AY4 z9U@BEq7Y7H8%}j2;X`?ueLYn#moG~X!pJf_B%35?1PMG(hjI|%nVYs8{Y@B=)}zv& zlFyk%fJqiCwj??%vB-7);2ZMnsL07QRmfWdi`LE{r?FQ^Q;x>t-6|PBc1lff z*Hoa=eB4vcBJ8;4odM#A0Z_V))G=Lpb96tr^TM2G`nU*DYZp>2jXg7p@DV-k@<;dR z`(T!d1Yx?ujVw}ZPcUd#?aZAo1kEN@fo;K+645SoE9#K7W0)pDF0vMHd#c`;6-6PqP?|oa|cqp9q7yD)|4AzWmc~iPTC5u z>sSx>1=*cK29%C0l32$5k|YALT;q}aFN#-`tVK?i>N7?$z{|9I<3f$sTq{*kf>diO zc*L(iCSVNGYUtj5CIhKpEc*PwKh1%VTP~)g6^SYnz}EfFNC#LbPb!LAd-8%;an~pd z{ca4oYRo=;jk!phz`|vXgN~YLlOgG6s*6yp8J8IChLSfZkl61UGqpfj@gx!G*$#B0g2TRM67_PbQ_a+!-d|_}l7?hDN6o zp-U9=9$v!x1Xfdgys0FIXz;;(JPmCNn(mcw3& zf%OJZ#?=6JD#-6^I!<5}>dn;Dt8AXA_P3Sc+PgpuZfBrAG=K$XD+GAkJJg5y@O$U! zDP4Q#B_yU`9VUcw$)b`98XFapMTr_5o3jPj?`H1VsUTS+9ignsscfuZxK90A&Au4u zd&D+OXAu#dVhcW>uc)%|`lJB{0yJ`IL&LELhudga%4e<2s_u@615|iDq|o^@fO>QC zat#Nk##32N5>fdP(}IAapG9a$Iy3Bb&EsrbfiH-q1zFYY6%Zl`+)Y=Do9fdAP zH9g;nE8y`p$x{&9h@xqKsxPnotx@(O4>kbs;}#w!UNX`XoFSL)!(>wtB?Li}6wIMD z>GWM2b-Eeabe_zfGn3;&I3}cRqN>j{m>|9t|C+~l5OF!o%Gq!Ht5v$yh>*`5vgM2N ze6EYTrDmy1PR$?QG6g24VIfV`l zOaBVhWO^@v^!_8ELeFO*x}E{S?6;{tggOwr7Mu_86eusT3w<@p+`?&BOV>Lj^4N|L zu2NCk1n|d%yZsZ)zMQ{gRK4EM%j|kS%byZj(lMw?rrq`6T3J0iK#A_xRUUT9$!E6g zUy+07)L?}~Rrxo}BXPaMk@)%{KrzjM9h9Iq#^stkaV4q}(Vjtk0g*DrWgX|El6EU> zvetbd)&YDjLo4d}4pXkE%n~K(K5GSnEzrnqGpIHjPAjcjQ$RDCDs4Ui%b_{IJOQ_y zcFEhK9Y2IPCJ({j-Z~ISO%W02Ao07K0o-5L=UL;UF6`#QRL|}OV zJwl?4`RWmr9)a#_a@T66sGb%#D$hQVT2&hLQ|={?3iJ=wV_GD?T*s=kJ8Q53RR{KZou!Eq0~`c|^Nb zo?~DPii-y8+gpLu`I&i;7WQat#$-}+JOl3JaEkWY55fpkEJ&&z|FE>SG*a5=f%2Fx z1y?trFr;gwu`6V)VR;x;?p?Jc3CR`9j3Ak_ASrp_eUJ4$q`j(#xHQk$CE-}(fM6)w zU2(aDRr)s1YOUhWduM%#&4lFPg{|W{g?f#!VPIsEMsBh+&L?!7TMvs@v1TjYPz1oU zNXd9jI(NyGLM+uW?~@Tw{jQ&L*29* z8wiaQm?}e&5XmF7;;0fQpdlv&%CCn_s<0zg7&bu)(1?Jxj6h(Me4Yt6Rq*5nRWlZ};Ve!U zipX?!heA%}2TNs|@tTYYv85)o8#4rsA#S<7a+KV}NyCtOmOfXu`aYt4f$Aa{5u>6P7_~|i>tFfe1Mk>} zxfX>m&g18(+?W5frMJzDP8{knwXIM1>r&0nY7Sg-))_tEx(F$^JK;5WTQ{ukjC~dq zJF3eMbicx%G(=kMwOPW%BV6Ax%tC!53FwcU+%@Kv48sP(34yqD)Ah8Yzcjf{cYI(l zxk4cHq~2M=@R7zB2#}vF!_`3n&RF%E+5uR5PJ$@%K6(M^3fK81{zs3O>B~5TfZ*>| z(SDy^kxlqciJ;1PXiWGmhPulqGZA74<4am=P}#Z`mgMzn^W15qD9s~FHfTOKvqWVS zs%p;0es<{W1k?t|$|!sNJguI#Db5t}{+2;RY*ii)zE==l*W#N!`4t7kYp;c_@9Q8P zjBgw%<8M33J1Ck2xHJuLYN8TO_K|ND zh!64wCTmSM{?CA4RSW8ZJl@9aGldGC7}}zXbP~1fs_x{Xm}SGsD+7H)1Vw82Z9nFp zm#qlp!7UuQc|z|^)ykHgA>9sjeHpvi9hEBk3l}Vs$uOE-&0EUF7c*Y!Y$i88|7@{1 z;_84SjsA#H@>?PDlYL#Sf~ph`)v5hG!YPe>u4~r-__%Q=Y)GsnVtpUdu>dknaHfG7 zruyh~>6^0Puj#g}N(8&)j1hkRLVu96>`&M0ChegWrxj(0^a94ZTS^D$=rjqF-dTS+ z?ofxs!yT_q6rU^lJ|ajd87Glk2uc8I$hl%sl%0WAbKWBbrK8};p$}%p+~o|-7NBq8 z#XulYc;BDBo&X*EqRqTmNWc#|l4CrH9}~!HzwJ7zkdD25K-F^#z4);3B7999%DLcWbu6oA6Z^s7&Nf5W1=xs`Zvop zMB2ygH^c4&cw#C>_Gh=QQ7BxlMifh^RDe^W@s=hlxI@+SO}7Wdn^VspF@#oYQ(flu z9?A_+>COTM73p9+owZh~8hI7{6lNsR^JcE4wjazprfq?lYpJrx5||X#f}iRLRwxI+ z+pW>@L5r25`8Jbg^cj18Ub=dAzZzWrB`SrjV0K_at-lo32dapY{AP83?d)-*TbP6> zmCo1?`>1w1pAVSNI2PC6-@;b!jcZS3X}L9y(y&qv8Tj~aR0^j)JA5T648$F?NhZ(x2iUo{Ev18TV!q-kJdJ6JASML+v!^6qQJ=1Q-*N~y^J>}! z-$ZV{>9jp4=DQhuvC-71t^bZ<5cuvSJHvYGC+Jb4zwytn7B>6Gj4Au4+)cj***4Mr zLrY8F#=6O>&0v1{#Akq)RV|AXi{XP=uymGTYC9KtcUA@xkG_+C$DqG_F_%Gl^Jyc0 z0sQ-eXy}DU5U?nAxX$}*q^GlZ_w15btQP-J6)MT5Z6pEHRU0h&v67Q9Q6 zGEjQqUDj|y+q!A32GVBgrbU)-&9?~VEJ5z3A5;{FhrC%8?Q*;hu@{#2X#Tjobm#?s z&7?wi@OBOPZo9g!#ph!@?sG2>qq5$R^l35djalG8@lmC&82Fj9gwgAMPmQW$qVi?& z&!7SUNIIfn&wly5?dFu!A4iH646N4plh!KKZIGGe7$qVlBdShSjJKVrMSfnNN zBDi<>k|}f_$Y#h@+QMTk_M&_PIhl%GQMbG#GxMa@C-@?)twV>62cg7+0f(v%UUi`b z?c$`j-L+vB#(T~-y2rS%e6C$@hi+~_QS=K(qf-7tH zLS=t#(`7pqcNk>-Jv1|jvmtYvMQtm8p(h708Ec2Y<;7B){BAk8UfQde#{UW+Rh#dW zroF?))H6ZGDf9H^i61)~`1NOSG41X5({Uik?jFKU=wM5Ho<33hN2p{ul8h5Sv zM)16`I7NN1m+6vf9~i4{wwYU>K{Uc{a;HNnra88jm3fLxqJ%+3c z@;c7RAPf}ZGB}}8&e#nfDUzP}Osz5y+~-A*{nT~*0W|@WGF=>iPVz!jXxF`kreIbZL+B2Nvqo*Y2e)uTiMO|`{D;Ss;-K{AOD;>+| z5K)ypSsdIYcv&|t8QWM7*j<6g-K=ru=Rl3|gP|>tRV)+kuAQKDRQjQRH$4A|x|3ay``4{!1wr!9r~d! zuzF;)CYy&qlAJuiLt^PSa*PGS$9Nt|LsxF}58zl^_*Y%A@OE>F1XrufYZ6=cdm`Gs z(A?>D?DB_U)laM90z(`Etkr!{1R(jU!mD+DYbXhD;*PQ+@S(mbkOgDWCG)uE$$2zQ zh)8X(T3m1O)6}Z4&t{l|g)BPg-*@&Ne_(gPAegQK7xc%Sairzcn3p0tV;a9(xm%K| zKydbo3QwpQ{xEZPxJzURPJ?nu}-#X5I zttWSs<>L;ro)wD@mBZTytorfP=nN|xE7U;$9k`AlP=LwzRfo!%DQj;j9<#8q>p(to zSqRarTTenaWu`gZS9(DNnI_L5mYUFM_MtdR&uGvA&u`tlAl|yQ0tW-JZj%ltF1ud_fs39z{U`Akcf6ARDBi? zzRQBFnYps7MyNeGfYUz;)RpRyrzWv`@~REpi4KNJY(pgN6s)r%&2*J$`x9c*XjKtz zsKx0~AFy15&=#QMGkn|lz`AosW7ZR%c_z4=>iZOYDL?zC`*t$88L~T7-&G>zpT`k3 z8}eCoC1&MgE&8jm+W3`4yX?Uh6WCix>eal$NA`VTUj_oke5KIs|;S+-a)v7 zkOGR!tO+``S+Uqa&%QOz5u#|>K8&N_ZwkMzF=*o2U%RJAk$0Sw(dJthwamqm{py}0u+p>KW<-zT-1xz5=pFFKay zqdo7twZYe~Eiy~QyAEo&3hNbx5JSonqF+pTITe|CNPULPH3!sdzkMUwnS;T_A6ih; z47p|NQnZ2oeq-F`D^to7fSnBcm~!5bBIKS}Y*r}DN0Z`ol=b@WRxTV$JNnfQG`bQa zoqZJXEvWM7zf08PcuGqNi$ancswzwn;&%S%C6G=QSd#d`KSjopG8{41Yx500=^zyx)~|c c62=!i-O{5HMY`A%q)oaWcHx5EZe79ZaKv-K zFi0~_)oOROG5uHfUv$E@M){%QYqQuq31_Rx+$o$;xQ@ve?MaD5daGaz{1xn1AmAOW zVtZQXcq>4Bk zvK+GRt4u0`J^4iXTm}3x1{UmML`yBcz=qSb>^Z@z|j92@X`g z-D+)WkNu8&%lg{QbNBK7aE)F~`7hw1bIvd>Lx_Pfa!a#diiWQgz5Fq@_)@=LNWld1=*wb zYIr_?+=Xs(jr((mdSx-~w93WesS&@7J3?dEB|EZb)y18wP2>PpN#HUGbPHAdrE+d~ z!dkVHZd>cm=205AImY$``BdDQnn$u-I*kFi(7ApEE|u>PGIUpcfVHA+@3wtlmZW#C zm7zro$eZsZqC(t|WYqCIg2s4J{%3KGcfX{d&z!2JyjlukgqdPv>EjTPHPRr+-sw`V zO*FgW9_Jq9B85VE2L^sLi_Q!mm9RF>SIqK<3AIUgw0BtORsDQdnrOfRxbJ9ZDOAu-}JNpW7b0k8OLV~0t> zbCT3m+f;GPPR&8kr-eD z1Dx!+agjLM0J+l=R7*2F07pQ$zb}NGrtx;|$EaHSfOf2=@H{lBNqoD|Y(xH>98L9$ zX|uW#0a$BBg7k_+LW7djjX*RDhIn&NAKBcxKv~J)UHa0xJi{~jrY$yX7upXu%i+vG!s0%Wp!=*l2~w3_Ll z-!DS~Gh4LaEup(HPUy@6ltR|dIoAy_Uh!{-y}03pUw((mqg1toMYmPqdMvRe$_Vbn z64b;EuB6>OwE#ARv`DECU|Xzpu(~?nfvd;uz9D0DMF;p8e}6@VuQI z<^jaj9!<4HxQGp>M6&0|qH(eV5b}j3&vS-5gK0#pN1sCnR_&8i*OA_}%i7bFRK4b1 zXebz5B`k1UuE^|9REMf9*TFopu-5CH9Bf?SJ)B*1v12B7KDQs5PO1X0e7RqtqS z-VmqPdLO4)?S3r9bhoWI+;Z%&oGE&VsZ-?1_O78v73uciS@g5Gk2DTxK)45P3h-!x zniR@Dc0pKM!1y9~9|K|BdJ<@~r~58Zv|55jdxSG1yAe(7`vyy9Qj-;|RTuN}TMZaw zLu(1_WqsCcEcsLlLx z!#R168Y}b6b@mJSDDXViR8lP>(I1EXs?wCbEN80#k<>VVCHST(Q4=6qZEMUB1RLrL zI%W4>aw;U^Q2j9~w6sW!`qXeg#g@dOk5-~d*1ufB+SH7whe@TI(rf<_{BA9TArNzqPxRaPp5<8C!T~&?ekaiI}cq`(;{{uMqD_9JT z>$Qg~1f2L+wsSReB_VoD_@%|JB z@YSA}4d$jS&v$!V6ZqC#v9n+FYD>~PIcE0i zd<0r_6NZ*POXYqjvn?i{o7H?*J%UcKm&)oLaCMCJ?Yu<-2X4z#{g$uHxWtw|nIOL4 zMtv*l4BD*5M*)sXhK0cS^35+a97f(~Y-xMO+bRDmiH*xK@C@KKSrHLivdzj(C`x$6CK79ppY4^q4d=A^*EN|=q!`YZ^@ zRSK~163rxK?q`R_V85XbG&C*9EZYO$LC4wqhJX}Wy?V_Jb);YGv*+Ssitrq-UggOo zzF(7mGv~bXk04+BY9F5t^Z1{}&dQ2+L`b3PPnA|3hKsP?b!gBCHqd=x1^K55VWYagXa&wM;;%@OWDHv(;kN*oq5L5naes6Fw)Us%}WJ4h~w;bURei}I=IH4 z3-Py(qJNvsdK?yRTUq3b9lLpKf9_)ck&$YCvm9 zU-{zktas9Bg7&5;ELs{0Kl9xsXFj1q!q|AqP$kd^n)dHEf1(HXpnPyZDSPuMeD4wg z2P)w#iB6SKRU{0KM+hP@xbpbssH@wwFl_ECjJbXg{wht(!}u)N4CZdlcw?Cjd5Jdo zGnOmln~)^Y;a^>={$&sF>RlO(UeDL1OFr&LKmQ*D-@>I#kKE!~lOn|G5+>~qR7`3b z^&ri#(XtQ}yD4FmfJ`ri;SfBFX6CQi2;pcU99ZX9 zGLZg+P3&@tMEiHx7XVYdT^CJF0af$w=5-R=IM3JU&7x#Ji;~YGJ6}Qyb;~MDJj(tB zVwFD#vD6xtfj2XSSJVvP54te8h&_+Wm=|B@tiqTzN6g6&uOTx+0JpF82PkcQA0iNt zLB>p1$PE@2JH^ zlLMpu@0BiC%;s&+I}1ak9|RifhUyD=v3(5DX{_vd0h3z8`zVGdU5m#=;-Ce+ffEGD z91=?|NE&*z+|&;a>{_K{5f`-ZiDD}zHL#{ljZ|xiXBh=dK!5R@ZEh2VL&b5mi6&YF^1(%!=W^8I)l^yX{K8jqcE`;#lypged(d zanhOQn{av8tGosE3)0$S^RyLWNpgULwL~WgQP3RhGB((k!zMeStFDmEz9JjkdZ8t0 zhB+(YS5r}Yd9Q|ZkIyUaN=;7h&vJmL1X@}pzrEBWlP0L4nxEFQ${=VEQ#L%~4Qd_C z7}VV-_22cL&hQC3ZAOk)?r`2v(kxTtgU^0b2RnTDp3`|C56BHK|DBf;+n`mf( zXW&ZASisO0Eh^G|9YD z`%}lMuxAtYnNcrGs9PVs&X}5#OZ=j2cZA8wE6R2JLcU7g&mB7v&_#)`tKQ@wSK!FK zxR6+P)5@6cj(qMv0v=aqIr%G>GTQ-5fI^>(=BCdA6$KMa6JO@a@TWUo2kqb6U1kNv zgY*k(RefNU;eR#{U|4(6gT^H6@3!Y8NQQ=kYGV|~7oi{M95!Q0*|+?-)Fi;$8}_!K z2L2}YMy`fbg9p{PGyLKUjvt0kBC6S{2BG!32BFlY;G?feo5hf01)TcdaRQI~)t$hz zMAW4ik|yTE5k$%>wKcmjb0lrmGl?MWi(RCS*-024BjS3ujs$k?-2Ej=!B=qKKZCUk0+Zo%v!e(UXiV; zfr4WVNmCxqn$-|aSt7N=u9aH(bl$&jCX63{QpUezH2{<4k*Fg9FTsMN!()=T%0e?g z`Q|B%fe6olrr+fq?GbUuQ!^Wv^!dVm-rL!RYX#zW`QbM zK=#)R5&u?%&d~_YSm&rnt9u@C21@r7YjD5@_RFEykI7g=OzcY)&l;*10o5J7lGro8 z^f#Z9a6h#S9V%04jMvcn)gKq{#OUtOVvawcXjm1AuhjL$$QGjKnh2jhrcbYm$#QZ> zzJOD`HBXS7@kiVdc7*btUu*;($r>c51=BlVaP(NPJDBY13uL!Oc-wkcJQA1Y+)$d` z`mwr0!WgADLcclBE0UCwi3t~{UBo$#^F*KNyx@$XFw0ek4}*5C z;2Lw z8gz_dIYj3+1j{B`zsa<2litf1hsgqZDRc@7@C_7hfv3hF=Do&<@`RC|O9ClP3qp}n z2nQ(~?5_!%**ex`n6s~_lj7U-9+9*Q(X~Z`#LlOA0p&#Y?m?0Se}-3d&}=gM8^J4+V83GD1c_Qg6) zY{?TCP7K42EDr{>&}rx4wO~P2+2;;3b`i%D@hEr@HeU9E>)6h?2-enxVev}c=7p+- zPJ?)Q9#?|$mx~1A8XAk^u?P#$9$77NVzg$Lb{?mYJ4Tu6S6ZZ#1e1=(xcQ<$FxLPj5BFgm;*gTf4pr_n_0)pzIB=z zOpBv@FvzTV%Q1?7+VOvx%jk}SZqRC+G-N{qpp&GZ$45PY+pZea=1nKgS8fj0m@faW zJTpF1OXD2S5}FXm`|$J`8&-z9y$ic}3t{$bk^=c=!0Iv`Y)uNpujml?302X$Fd8bj-l0d!5WfWk+s@;LO(Ul^k~?KVtvPOOz;)hw_Mqhv0L2O2Y|;_WM7H4=6|~H$~Sa8 zDI6r=-qdF#<{kq}FsZ-{|AJbakidV?tm}rU&>$&u4)NG7Hj9bl&w5ulL;}qUcAGgB z9$S18(xK3!4))yOY~D2YBmVQE3o=ya9^Q#`qBw;JQEa@X)*<~J63_i%Lcg{7FV3=_3k|O1y-%n1{nvhacBKf{lVTK z^cP^i4#wQ-&29y#6N9R^0{$SS`UQ&6GTD@ad~S#==;w>FUY^$y{O>8;+~3W)Cb=)g z$4}Xc)s;C7Cxy3`9_)8qS_gFFTp&+0wuGfCfB4fBVY=KiFv^oNFPkd@##OKq^B5h5 z4=iT^MQ52NDn_JnR}#q5Z)Lkv%@~ip39DCvJag(L4(*W-@Q0-6cuE zQhU1v^1bkKSv`Tur$En!A`pt2i?YJ)`@NF0|GlnH7_^G->K+?K?GHIobgHQqV-ldi z7vT=}Hs>}(Q0FueqDAf%gU4t5{Lk1@3<3_r7-NSwSI&TRED;UNHYi(FgzVpD+QYwX zrdJZ!<`x6zcqKfWmCL4$s3FjE^rvRN zYD_p=E_+k2JKtOTcI!4{Lbwlsv&8gP{9GBDn(|ZrDU`evn*_p90?w+Ka@md}gwb4Z zkW7=n$;V#gY-MA%N_&;@C+Ao1Xkp?_+&8jg-Nj-6Izkdj{3K^~V8D4&(UiY8?g&-q zAnKzNM3+sFu9Pj$PB&KikPFKAsc+i3jmcmB3HV7#`d@Ehkc0nkdvrTr1fvPTFyQyp zCYREa8s+6g2V8%RZ2)RTA2VuNYfW>vSXXyj)l=}NUB)A0=#(GmpaB-f^l1%x&A~Vo zM&xM$J&bOx{EuL3%6{d3QL`X69jc0QeOlxSW&Nwy(WSFx%gWeJn;Xsx1E&eQq(Hvp zZuCd?r*YTq0@rx|(@n>mb?0-J+B^w#*!Y>EIpMmEbj_(u3hBFmMB1&RhNyCs{#$P8 zqnY{mu1MEmUZZ}iN9IMM-h6v@e~%o_rvka4b)A8EyR+`<7##+mG(A(;!I}60M5h=6aMN0*YM0Y)pdjB&VfPv{N!^e_nwdqq7(-}E+ zp_alNY^>32^@kYd?GykV9@V9lvCAA(=-axXVw46h^4Q4PS2KX`^~CTsI~@EDCwy3$ z7;t^mpRH25(K<5r5BmT;uDvgUTy4$1U;G}nc*oy6`KJAho5+!w^TV;em&Xh;H+6ee zgoo>mOXsW46+#0WOQNf~X;qNmL5dvJd^!P`i*W87V?OzU7m)cDbs%KO&koaViulF+ zzFWQT|1lsMy<UF|YFHibYH5?smvf06ro2mHdZOCrcVk{*rUO2^2x z^L4XeBV`hqwQk6pVYW$==2CmUWEeZ+M=C3N3$RKd9X8qa)Dy>1WR&%HV^f14$}IIk zFcGcP3!mRqU8;Kk?7WVnVL0hGM-uC#42&i8z679!yx#g-WUb;aGr!g0ryO)7<#bx) zO0YeH%$Sm!K1Mn-1gp`HqE`|R9)>j9EGH?~Pkola+7w133`Je@vWUE4`NQpjcg19f zcO4OHuLc+^p=s0sOGb-T0UrDPx~W)eHe-u3HG;4U5Ow{z9B4u8hyl_$2Nkd_=+bJ41f=%7T_acc{HZ*Zm*?q40Ep`B9yE)za3p^l*|Bf8o}+F(SPrgh3rHs>5) zv~1_5_E$Q{-(dLS>0Flb7?$O_!75&sW8Pw^o-Z2h_w*PX2ka*!HcgJvP+tLR&MFT3OHgSf5=B_kOGiU0KV|r3PJ1d~-RZG-4=a`8W%&fP!5J1r zE-VsBe0~~@n7pNuL5ie9*+2pHAMNJV%g;&e)wD;NS-hwK&#SG8@O%W$o!F@YiB7i^|pC}@s;(rwo z5g!|(n?bpf#~c8OGTy~(bZkf?*t=5f?^;m?YC*9{-L9$$KWW?yOY?nZML--_W=+)g zZx{Yt##7|d^pB%(+Qn>ov`~i17eS@Mgr(hV7|*5QULY!Lx)l&+poss8=^i3XSfw>M zeVj2=UjN@Uw3GfO45{~df4sBxhVJij>4pBY7(VN1m;`VqjIBW?bs}jezqeQb_3s;u z7bVUw)X>EWs2=*jV6m881)sZGj&~DT5yA0y<$K|s(r?XG`y}`MNyM>gWv>1MO3#rg zN()nnHd*do=l;P^v7f9+JLe>7h8b8M2dCOI;tj4p#j6?!T&YNhVTR9T!U}7LEX>~4 z=1Xjm^cw%h)yc z+`wbJ z;C(B=?C0PN0RQJI+wp~s!#BsL(LLm=Kv;Om-w=Tkgvadevw>ME zN{Cur*ci-2_gTnA{c^agR%l}*g?#=zSA^K@yX`vo7|q027_;ZT<1Rvb(?bs%+-U(5 z;m|bL9TK)y|8>ZAsC?zM36wqL_9#x9)UYlHmN zEz>~LNa4WQb3kUdx9nhwIy-C`$Y=O=M68mx$%|XvVZmoQ zu-suB5W=OD(LI*>%IHO$%XgAKOTS={DOqXsh8nTA8_V9{oMQ{^;;^whZ;nR(m#{YW z5gLW3s~(IVebFB#(gpY7l)szRd~I{HR^L|sv_Z_>%eG{FzcGJtko;qgWm3WgKg3y? zl$ul<-fIqNnfwvTe@8};-HS0Vw=vG}k*Q+QYmC4tsV_hMRyY_cietNQ8tdEtCHSWV zB=P0_+~U7IlCDTj51f@oO@?ofB!$L)S$#hh@X}d&QhuXaEq96e+XP&`*--Nx!Bz_} zy3})TDgJGnq#Kp?ha}i9UmxyOcso55+-SaJtcTJxr*+>f6rg!-vdF=j`oBgl2;ND_6Y(k7yULqIjXis>!B#(5ne|I3P^1 zrjRYgsB5D>ye*c9^LMmy<2o9N zOEXwwBS9_rVDGPT4d&gfw9t@PS_;+*P3@b`~P5|>NHj!!rer2ys;g==^;{mD<6 zf1xV>a=kgy*^We?-Y>5-fw_7>!RC-Wyh3NY6u;EfdI2olT9?C29nDeNW4jA|7MU=H z&kl^8Zw48A!)89p@Re}or)CHL*{Y-~20PLpS!-x8D1^d<=GO5MG`VGSpC15dc2)mW z5l85!?n~-Dr4G)B6%^1BP6b;Q?aZ~4n9;j*ATs}Z!mO&%+W=aF3^Lqn4x$X(fo;Aj zaDgra>}feKD1=Ja!!(-{F9Gc**^p$7S(1Xue50K%)b76WK-`YVFyimCBj5W^=H$;K zCVkilm-lkLH?=fljXsuQ!cGAp^Jr(I~WWMtiJLgj%}m{*b>zpcTt-xOE|lg|>S zE3)%9iauvMS&$MG6kf?STHoLsMr`NGb>&xr$GiA!Fqc*hCATrIE#CS~O=J}Vg4d00 zXhE>@zvcw0+54H{*4`s9WP_l}KrytQI5*~63Xe+SGYzVrnZ{)gBxhJ`Er3zDBi}!m>?B<#oRS z>9pO5`zla)la0B_D{;bjXBs$riHD4si%;+?VPE{PLZIc`ioMkwlOA%br&?L~) zN2iXiv|AynKs8e|KUBU?Y6UgB0oEf4(VYNm5O|2RtS!JCJu{hYm3RjiVsJ9*(18cw#Wlj zw1EschB=vF0tMzC%h%2e)hJX(Zz{pxJS_Pp#zE(P97!OpFdLN}f@uPLyC#Sx*O=FDuiB{vO$dCGFR&`~gl=CX0HL@B)ilw?ts z29U`^QcE@$oUS7x6!b=S%Lj*4$pda|n%{%8p&{ll(PrpNPXW~}*4xzlurJ}_WtGyA ztm7dLps>*f#3w{Bk(jTqlGsQR%yj$a1P|SwRo+RQ_>ZIG6^=2%Y z($tN8_2=|prptee$^U8Z7Z^J+9l}x&5W>?_Y3UDxe0QV=bvopdnMgYU$G?$_kdma@ z;p=;aO;VGoG{2^}Htp$GC4p5Z%p=@>-pvSy$D2|D4}2+zSIK7VH-WJxpL{uQ;+ofy ztOA#Dd$C;v1Rf@+&pX$HpSYEHejk<2O;Rd~G)^6u=fU_nQEXXR<{@x+07noy3)RIr zQ*>4?|2sC65TV5AfgE}_RdIHaH94ODE=VlU_#T(gYebXi{&LaOm5a(EdKii-1;6T z=Pw~|zx{%M7@_qIw{-3b`mIW^vgg@q%tgbC<|o_qbNF^%a{qg=ER5jf9xW_X(|@Ch zyPZ`R4n!Lg$f()3d-8MJo7>j0mYvpKcuNQlhNho3lLK?nrTtI zy}4m3Yw@_ut#58@m&Q9Dx!c%0dEwhEyPO!Bvw${-3)e92iRqQqREORLL^dkM19$<- z0KY9x9nTIn>gzOTt+e!%V8I^o9H9@^}-cH^(k zRi{?7`KV01$MM{dL7Ff#2(DE~zx}{IWcCz8&@*sECgd&r+b7rja(g7e|GD8vY4c`)_q;@5TwU@zTQB?#wg zLjH*y;9mH(pl6&rNP<^sNY<|~q@a>9o_(;4RAQoSzrhhCPK{X*3aP)@#^`d7xRp#7snI?ysWb$=m6CTgK z6Vkuo;}QbtN|S|3#+w1>l-3daUSuM4{^8*7yrP#9k?KuJ`j=J>)^&S}_aKNKN8Z{B? zKt_k_Viw$MIara4DMA;T0rx>2@MuAjUoT>ujhsudn)K9>EUbTaz3ZSV$HlwTcCJ$5 zRPt5gBIp_pNUaBrFkS}18A@o$;G>drqC1Nw4~c`kq?sO{&MC{hhJ__S_X-j^D?usS z^wKZU20&JaGuZAF6Y+0E^PB0H>K>D%VJyj6JVSc1yu?h>RBmKa@9nzuHB$S>P!Vx; z==_Khv^ZFoo`oFIQ<9(>h&dq(OT;{rR>G8&Qy~AO4uS6V&{5foIS+ zjatYO`np@CxeaDFPsWmZNL;g3DQ!CGu8^K(d1kDa(Hi73vt#mK__X9e!&I@bW_%H{ zbVeo`DhCEHU#YxZR>|9(Bg#A;Py1$EyydNpdA@LT4>(4&oa-Y@Qud<$Rh;7r`)%l(m93zl|TuT!X>qdc569X*06|Z?hpIKoipvA?Qu>5(ttefJcjD089a0u)J7JmRAbfF=65D-cz~8CQ7yfWRVsxKSSP4c1^A1sjc_Y`Z$@}9f=qLAq8NZ`C;6=FyNTj38 zZ#BjlJ#@L2jwQ-_ThIgtVo{F#L>sF1D;No8n{p~(!gmDFH8%}b!4XPd#gZ-F@ZmjP zo35!`zK|4-%H3Ze1@TZ|4aw#Y>_64>k*5w!=MD~y?_;SX5R6GS%)^51?$aqNkVGh> zR9g^9MjyX1z*LzpgJv76B*XbF8CarX|a0&k-tYA<@*24{=Ili_P7k5@eBenMGJbOAEn86%ms(iH0*J2KPI((R6*Vz z?w|K%;0$=rlCzS-`1qXtF_9_UrfyT}yFAG%a!AdmWp>X03S#f<8~xpH%OK5CMU^?s zeQdefcr!CG6MRXRh}xQ4Iyl8`)Av?{OZ_tu6(EkeVjWvi{w)+A3r#6w`n}aJ+aGD1 zM$K4trkF2KLbHzYcgUhYjN8ScLPQZrtBmct6ZW|en>S1Cu_j>dcXK-r?r5%cLCi)V z6lv92I5cEPBL3bWiCs}E!0?Qj)9@pN%u^-LYAY19=)pwFb{9oXh zy7TWfX!INGItCc&EeRhIw64B!bkUGE4KetdRjGkeEz2DecYLV9ZW@0 z69yxmM|9TsxP^Wyci{j~(~?=Ymx*g2m&CGefx_?7l7KDsLIskaf0)`Mf~Eq4HD6j_ zo`oc{w_#@NIw!MSg;i88j}o!NXQj>V_8f*RFs}d_y?fsA!oGoF@k;e(rR6zPq5$MB zbjY*dA%hHoyN5`B8h1&RRw_^`Ef5e;F7AB9Ol@@}dTVC!rk@&wN35{rH2r1DWH7?X zb{*&BW=eQsdp|C4hJ`U`E+L8;7N3nSThMrs894&}9ii$bGHGVGF-8Hde>z?8JIw|8GJ;rHW zH4_(X)#?c>Y^Jt58cYR>eNngo{<8@ZMBz6jwf*w~trX>V_laDQtHUnyjl?>?z^Mzi zq_oZ=Q`Eow^!wvYxv!bBFT595)iI@mco;r+weSWjP~;FfUiK#IPnRaju0F00OY zylt;FKTkb?l$0cj9z+SQ@B{anJ0R0ss*x;k=tCB;eQ!M2!daA?^0DaeixnMh)5Y*6 z0-bf>VFc$xX4(J)I~?#~v@+Jh(=AetUFSz8mTjCKzPXw36Y-`anRzB-^GZx)j|XmO z0dFBjazeRSzRK*4O=3Y%)1k&m z5Xd_oq0@15KI-`unW%t2xvUq7sIWnGUOI~@DDp$-;bA_I$9{|1gv0UE%A=@l|7AgT zW~iO@|A;#qUORg*u22{yLh%0n0iEjsWo3i|8LMN0G;eGUfVrU;vTUy+y%8F7|Vl zL1OH4nb91KV3MlD5f))ZfQETOuYHXw_6hx$`8kAlO9Jfsa+r)9Iy#pZqB*da$SH)y z1A9s`wW2XUPiJ=eW)JK0!6UKgeLI;qa1+JkMUfLZDJLlg)o+mau!|e;VZ5H6k~4pF z;_D7<)mLVcoM@gEcU-}CyNA>ZdnH3+8X|7NXt6zLr++z+e`1^SE8B}(R{yW)E;6zM zy&r0ukqY}|1o_oIsf^>v`%n&zEapSHBcZ>ro(a$g{d2oi)hSSS)wgB4lx?WNy}TkP zGIuLn7r`7*CvMV*TyvBk3-gZ=tQ!Wqk*=UZW)oLmGZTihhK_vzekJw*Q0lg*>;LUS z+j9XUi8@^C?X6NR;~boDWd|dBBL`NR^pIpEvh2cBT~pmTK9B_D48)SgrXY%BfBv@3 z1bB%6@&`VR70rtiXDDtWR;X5xTXG{J73ypRItnQI?|Cu`m(WXjhK3I(ilSzz&W*NZ zR)mCr*?z=}-w&}IC8(r;i8DqK6bL29>2>l_7&wFgR|Yf%d4D--#hvP1GHl+RkX3vO zO8hHa#srW;k9qsN$kU9w%^UJ9Z_?>v`1x#xFoeZN5YsKA!v7N&Ge5B1P=?qH8W{1t zE#bu{{XXKJZq~AQokNuQg@a0D>pV6-x~SRoh`{8{mbuTKYI9(?<~PyR?%G~`l#0p? zd^}`r1AixmeHEU66zXIk`csz;ve$p#26@7ccw=yqgw0E{s1Jd zF$gZ3z3TeD5IrL*G}8EfSf5t8(*_FtzF5jS=#mQN>THN%zhetCq_I(zGh33VRr;X- zI`3}36NDP0#}cQemCOgu2bl(KmNsOD#k?idm?X#90@%%qy(qPn08|fkOU#TNE zY`!!WGSa=pHePJU?4t;C`Wj6XW*Xl>d|-wV`DaWDLrF6nY%H3l=u{iUp^Gp}+9Q() zVCNqDZeF}(U>C?Q;NwbMpKCQc5^Z9<3F7c2lz3TR^5q<_bZW;z zk^y^8C6((Y&5<>;9i@~q+NA_AuT3(;?EHia_eFR?eXBOX@A#0(UbMHa0ipLIp1wsN z@-BI0I672Ta1uiU6?&T-I!ZjQnNTM(Z_+ThAueXxU3z71;0x>!79Wpa!Z*xqpg z7gpY=?W_O(PB3Uf4i`@NA==pM`P0YrEVF4+jG^QQ1}0kEHFfu6;})O8A^KI>bve*q z5ui=mE!0e6#u~y(`=(vy3#24i1I=s=@Fb&+*5}~ejt7m=e%UJx=}cr;jluEAtA@J2 ziW{~jTl({r+kf!`b>G96(Q}bCRUa!B^4t9K>Ye}}o{q5L=C(;QC%L!K=5tp))6%pf zBF6D=8D%tUG4>3}UR2wu8oI!RKwHkmR42R}2E`}P*5TF}M!i3ZWP!WHdaaLG5+C^~ zo4D2XuY3}8DPIz_jAq2I`#@oJeG21Y$6mpS4yg3YpZ@@XqJm2NqP+VYAJXAgS2QFJ1#VH`sYw4y{oiL@r$Pn|l5rYcM zx0+WM;2-W>8h*6s%Hy!Sr2VbL-Ui$=kB>P`KfQU6+K%ucUzPB+54KdZLl12IPUBFJ z&eLta(e@`JlBMgw*T+nRhb!l+lb1#QakRKE#HdEXR4VXVqiI)|-1Kol;>(lTPES!w z5Yy1h8o;AlA}Jf>d!5ck%zY3%ix?xcF+8 zp-t_Nk72N9dOnC+5*zSP`q8n$=qI=v3lVjs+V#F-BoLT{+<~1Gu`@}!D4&~mh&=!; z5^T6;^B&en#=i<)n<0st{S)4WYF;2-#tiA{yyS5mfyUO+Sua*~1=i7S<&L6E;i~^A zq0Md}+;(R4> z`!YL5o(=G*)ZFvMBmfkK?K=KJCJFb3$>P82fPkW}>O5~j__w+v!~GfiFbvku$Agd} z0T2PzP&zqHVAH+p&HS@~9}gG0SL(AeWy7uTpoc+qbm4{%P_s0)txYn3ryE9ckD{Jn zEQALNrde9vX}OphHaM2DH`7p^>MjMSL&}OyA%03X-Z2l82#RkS!XiOYi0f+UBz^3h zMg8Af|EgWc_7$(a)SqAoBZYP)Ua1d1uMUT|PLI}E5XhRe0XL!GD;>Jq^ zu^JTiQA$KAM#btk%8QIqGXlFk$baOT;|;Seybro4Kej*AX*z}1lrRX2i=ZaWJt)zp zL21~>En)O%zmuT8MY{I`!yQ1vpJk0O{yBG6`YmS5jt&kRgkh3#*nOn)n!P|m9mkz0 z)Q7@c#)yh{CN@)}9e`C=cJfK=Gn3f~&bAgO3JAVg#p^P=|06(|YUDj^@bwV*ZPV>2 zNPurTxPRLob8Q_ppe$BIOgs+;a`;0=3Z%b4i+X@_h*=(0t*S~_i=M}bDI)f6ui>>a z$`%i@MR^fn35`~-t%072nif82#^ZLZ6Nj~?&71&zxeGEALJRetYCEt9BA9>mdQAZW zEYsqrXr5BzcpF+IJUEtR7h80bvM<`oK8HbFSkR-Tst3M^H+G5t+$_`>6nu>BJwrDj zvHae#q6RptMQYzdH*Usq7sf5mK%j=ofpbxLBN;YUiS73Y40AyG9{qlVDjY?c803D` zA(D9fm5U|PP1!vvVQ$D^=Zi6i;f?BK9IeqSb~rm(Lcj_QJFWi#FphrBqN4tds8=Fd zw>Z~$mtc-{LvZ*E;D~klP&?R_IR00K8-N{r{?)P$3W;3+HcJ{4u^X?9*MU7CPfzX- z^R+@d+_FC|k9^;^O}H3Spgny~A>yCnF0J>Vs6M4x)1rmr!0VE9h{r>6BxF~bDHbU> z)FX->+a}|`GB5J|ty0mw6i8i%eA9b#oz-@{}K-(1U50QBB!oZElIB!36w;Ws50 z%{7^!v04`&t42R3J8%9oT&;-Yh0qSx6CkuH`zOW}n?}Q!_o;JQEd9qaqev zfe>DRpIlljuGZjsCams;wsiM$5-5Rm%xllr4$fU>3kbFKtDi?cx@XB;Ro%FS8$c;Wrv$0ixG!a29&enD42PwJzMMQur+6cBE4B-hjVNd`fNlc*jehc@l$F; zfxO&FFrNgvxkVE~@#))P7gS9eH&z>@G+?q>F#th#W?_yzL_TSVM5gG609Qb$zedk6 z)S9EKdTv}LtRnMFZCaNQ`O1J(gyCaVzs(6?`Y${L5}Uc>v(ZP@11Vy-+FztS9i?uV zbl%0->Ej?0vNMmLBdL$tw}+C>eDREb*m^;W6}4Zsdf$o_sM=`M35@ZTy_ z6Ojmln>C4dvys?_YgS5_hMR>i;+sD*GJc>^m`7`;z`jp^u@zMpbG<2Jz_s{;yo*P> z%=iQ-t$BTvXI%DK8aBO&?inT8RVnt3R1wtI905A{ZzFI9&&i0e*5<|I#l5f{3gM4? z1M=0bgX;ba69IE2N`ofbx{xG}-cUvQ*doP$c)&b%XB>ve2Aj=*&+=$lOO%8>+(NVM zO%Ph>rc>JSVg#Wu00000F2ZZ&$&(H3+9UUZRpqlW&BNAgS(Vg{i4VQbm%?0&*UcdR zs?_EA3V1iC(m`{8z2iiqxBvhE4(%c=V3EYeiO+wTY`x`%CXAR?K{1~KH`k|WKCp;G z0006p%0$J^*BAS!6=<%o4NkLe7mjm4!$4MqdGF3v0dFM_;4KaIyo+g*j_ zsBTAqr0}{2o%d8C)V_uh5*@$lvxjCNa|ZVDCZu6K!3OVRgN1|ZhaQS%OwHPG(;(Y{ zTSxeQcntROO^d?IYt4qY8FCF27~;m)T~Fn82*Dg@*;kvKS(6=UwlbF#$&u2gjt5_p zEqMhOC==RLBH$1=q%fjG0HZrEbD<91z+w{OlR6h~0IaXB_dZsUO^D4W?F^9yAb|U4 zi4JK}%DL_lYqv3+d~VXv1ZG$u8eEVZ z$Wi9a?_UCeoCni@O`wU~sAsN%yqjx11!n6;>PCJRfNDxs&epdQ)#gasVJST_5jw4d z!6w1N+=Y{DK=l3?-tKJ^_D0GY!!}=~SOEh2!Jvy&__*ox>RJ{_Gk9?&7<^E@_C{tZ zlab1Zz|f*z%L>RZ5Zh-VqqIQLAyB!uLS%yj^W*5$3+w zr>{u_#7y~XHSh=dS^AFyHek1E4q_SW`;Y=&x?Apjil#AA7_kx~#Jm;Vx)XP$bN7Q2 zRHWsx&v$Msv)*HRvJrSP|qdT&x%!wPmWR5Xmav7`wpC*LwrwmvSoUPlSz+e|_@?>Lnl+6A44m-;aA za6AXX6+U9H0r$VG5P^6h57*fEKoN{&+qbLNPM3j1_s9J61H9+hnd)U^u&V+RSxl01 zbZ4QBy@JhHZ=DPyD}7xi{&t5QIK~WR6rlBj`H;C6c<98N7j>zH4F%wEJ)&%Pt@iT1 zvMD+-w#5p9R^i}sQOi>Qx|;rRK-ask6q0V>RnVSOzp@z9C{xHv>Ppl`K6qbR*KV-Egy9Z_Q7gwtj08i@J*uOet*yiKf&o0ZHdg@H-~2SR0#V z{UbeD2J9o*jlwhtQZAr}{fqH#tw9_=X-0dUDc9jp`_F*f2ui1Ax^-f9}h;MLFN6>7V}_r)+wI zEJYTtZAsycf-Jhu<^#Xq3G-k?JGbW_37`NM?=u(&Biu4BA5FJ_DQD-Dy4#!UKMB!J zqpfh{wR*~^7Wcv-Uup3k>KNh_JonBTw>weXaDxc`%Z~{4y=k6Ilc2g89K1Ne8f|cd z98if#tJBlDMjr@ef(gf*hW6(~J&NE?j?@-`Yi8yU59~S=>z=$)FC?|-DK}B(t};t^ zY@b+`lfvuVY@3H@)f3Y-kjnpD<1+?Gmo@H#Cx9u1Td$W7nk)KGygf|QUR z)LzpanbdO(zIf~XoWxBP>XH=03J|It{UPt?EVMyDks(W(A_Arl0QP6ZVf*xtL4vJ zH2c*B--+*vh)C-8N>W{LLw{LXyTIqC7Ni*+)VHNk5NOGTX$kvGL1nEcR}TXK=ZVZh zWNr~Nn8x$Nl5_gJCF687jelOXlSy+?XtS1pJO!dPi#FEC|BjcugKfq)%hhRQEf92d zl+p|Ku{gxN%ruFe#-I><9-ZigYM9uu^_D-BG9M2QpGM1x!iCE<+rCAho*b#qqSdHz zubm6X*T?<07VZeC4V`KZZo$(9{+n}c+rQZCV|Q+nA460%*<324D&(6gjFhoz)IK@c zrRVqlU4hd0x+6>q0O94j7`;4g@BP>uh!I&tCv3^<0yIwsv$0{-kXVOJP|0|>MuD4$ zKN`=5Sk)z`{8K2IU8(OP+;zdxX3n4oTr;(cH3R{eQDCQ)79^Q*hS!5y|GoFZv`B8Am04o3Z_kV(skZS@emarN)M%rs9?(JSD2 z*881PP}XFz7yU5uSS#K%MUI0_aIC!v6TCDoiC=n+-ViLjArxw;>8l9a_o6!aR+{Lk9fo z=hT!ZjjtVE#*#`NZ3i_hDS+u_)DzPliJ%1h{F<-QiFGOs-c6Z_IKh+TM`~2+;1v`t z1tm@zeXT36U|m>PvCKd=3uGlRyuVs(#TuclH*obKcKXY1_d4`qe4QOSfM?8&!SH>q z#Jw;jHtMSsR~x5JyLgmQJXE3Xhd^FPq*iQRcw6M2;~c4IV3oH#G1>K$0ec!W=TBTS z+qssnWE70^Z2zYvHG+Oa01SSHmN%<9IDE0eNFx1`hI+D-MjKK)7d%ZAKJCwtlu3^R zBwKtcUxZzL;5IYHk4pUp%KhqmNjt`cOGP}8KroEkVt(bSpt$hRe_P%vDMZJD)TDsW zQjm2Ig;%N9VR>r1Mq8)#_&Fv@pf^9{JC{yE=06NLyVa_B&S6IjyO8m-11RIrZdtLO zBsbr{;DpceaGsC`Xf*3A#m!+XSSE2x$u*lc=EqFTwIG>^t7m>5NV0s~v_70CfUnP( zi9Pv>7As-};?d->RZdi_XRYX3WjVt~CJ|%qZOfvf*CzgyfLmf#TW?OEIC~-l`=^{j~HS>-%+P?Mk@Vp>ak))2>l$Sqom>Jtrf(4-jL4CPnR zKG(_En6U>NS6-{ZOC)9}EQKT3rXaPgc@hG-_x|_V~S@PbmkvXxcsK$pFKy(GQ0GxLppruW|MO#FUtN` zSV=9eD90{aW}+{e>#ETp&I|9$A6|zHxtZ?qeB}@b*!FmHn0z~N$Ni-?-bF*Rx@U#W zh0pcY| zu*60A3GNaVzVM%7)o*on$a%+W4dZLXEVvckbP>P713JJE+|dj4T4byjMjgg1`30V5yr`0{T6*DlrszWof9L}MNT^wT(k9-2OPhj^@h zGT`+*1fEfkkr=VWaQKKHi(Gq37+FOmQWXchBO#M1o43K^bdKAuwcSRzz`zg^*%5@4 zN|iuJIaTS=bFfw`-s98{>SieRT7kz+sn-~GEs`eIL3teyuoO2-RL@)1m+bW+z51=x zGk0wm)V{E5?cV*`91bcxBs>fbQ|eTf1K*1L|F?DYCB)#UZ5lfLe()`+I2o`)~B>8 zq5Zfa*FyP+3P3|-rxKn6=dmh{j$|Z)=F>@0TbL3x!_Qi28*iPh;zr#%ts0@djYQjv zQ(UfBh;RA`pXYfw)>>)C2E!?DGstI=e8uv50NY$i*VT6b_%0!TAO6lH=0MX4Iyp>_ zdMe2c6!2{ffq2H6Q=4pf=!NE`5pIo}N;Uf(Hi`yjcNIvvfyb+jB6ZD0p)$B|Ui|#p zKg~macnxkG+PPeMGL~2uUP3k?aFsqlkgNIKhX-XG$wI}}msd(cXsNZQ3iMEUg}P?0 zB((M-$Sbj)vDTGGcFjR`3t&--Pl!Wr1Jc2SIE?g}E@{q$_&R7bme-#}&SY(& zjmzlp2CODsYZPc{#7;KlPR&XbII2O@ILKoo?Ub!zJWywZGS+?R!JeX~ z!RiKszGxM{RJw4S%T!5Jv&$mBHUgV4(3sdt8tD?OJdJ6p3p`O`%;cJ<#Pe2OG`G)> zY-@v=EFk9t$rF|xf=+Kl{m6JT&ctKQZPaC33dpOBv+irrewSYMYafm=wc9(jTTw36 zY2=6LGaMUHoTblGoiMi}n4DcV9Lu8B)ByL98m7{|!}wx(c+Pa7XUaGL%0J6r|C zu>B>p3u1;Pkgm7|%=)>xY97z)&?s6WFQV80E`$I804&@$vf%E+P%Az~(oKL8XTT$4 zU;qFB7PFWUIRGqbh3XF`x{(eNX}}#$;N*HiQAx@W4ad7~bsO9~@D0iZ(9jRWS@b#Z z6TSth&F~m?SO?w4%&JTXM=(65?o=1W@EAeY^Qqka8bN=fd)2B2RQu;YX=20z8|s92 zQqWAU%WR3QvQ7!$>_HtO`mOzg?T}@syf-9%2<M!??fiPhUa7~R*du`?F8%>L^D(;jrb=v!sH!H#J?dd$#SX$#dwdV}Z zA*ae-I)5EBO=mPM`2ftV!nHAI4xNWcVAinEq9}GjG4?1?HCaTf%+J?&%KLlFNh)hOC%T?R#m%3FP$v385zfU}WgGt8vYQF^5J8IQ3&U_$ zuk0LSH<4BUB^mFs(jy%+(9ZjYg8`RE&ZOrwsCUk9?HN*r&*n?Nxn1Z1Klc|L^M{wV zykCC|$4H-~z6xia4pS;ffin@Z5GrQO!ir}|6R$-GjL|?kL_6UQGa!GG_l)NVp!bg6SDI36_ zYUd_>@c%rXYi$;6G*2J02IG~$d;w<~8nH@_N3aIy(5m2F$Tn=(!ist#Z|_<{(hac> zqJ!Bdy71KGlWpD0&qh`-#UCW)yV9IWkJSkpSIknxWrC<{DYOUVOqBTycCf?mhif>+ z*>>hUZN8p~ze41%{Nyow%x?JE$)=*{?oQX zfzQW`)~m8v%J=Q*i8h<`b7u@aLMxQ+_2}hr^PW}XHr3%sJg_cYws(X#J`dJ@HF)x78zWdzoSm9KFWxs^w+mRR${l7h+;rCYNp8v@@{ zBX)-%Px=}sM0oxMRH`V5Z8YV^XSZDIIB>>gmg%tX&wo{4uv-l9 zkD&ZJH1DWCxr86TMo;qqP9!{?X=48wS-|JY;qs#Dc~WONXzbhMSz+|>VON~NV_5N& ze4;bK3*dI{F&j#HL18fzFX@3l7|6Dl$ycMYVn&R7tw?!hxt7i{=9ciI>ffCFt;*W?+*w+=Ti(|oDnP3I0OZ>x&KGcS+V7b%i^s*W@C8@xeI-2p z+OGT_r@e@u1A<6mBwm|1S7r28O~V#?u33?JjS&p2Nzw0MbkKo)8g1@HSN?AGx&2}bdA~1bD}&c z2k zz5c`rqv)gPh$*~U5;Bxs9@CA1_J+oi!Ns^Dczitv$&zAp-o3Qsk|9L-P3coQQcj6Y zp;R!0HDL3BwGQd*WnnnQk%2JeaYsgZDnag@$*<#{Xl<&)C{o%B?4JArIFo109jQul zC`)#kMnW3p*VYBVCp_Y4Oqz3NWWgkzq5D|FI#DA!0JzO9l~Ofn7^qnAu~>{~Z_5%s zi=?4a$}pd{&;%#SAo_6-Ww80OCSZ1s4CMRv_ZtykQ#I2t@WM9Kyq5k*Oq(&BN(WUi zAk`2n%M9~s+0Q$OF_7>?nrTsU6*!onMy%wT?%dHaHcm_HMS-<B;%Nu?O!)aF>6LaVhmGosrD+4 zlYF5-sN@=?7xTk-NX9uU2Y{$(QY2g@T^WTpUJ-f^b3@?G0uAV0|TC>eLud17>-Gz^N*i; zk2+%xO_qP(umWwP?_X`5kT73?*>C(;` z1?>n19-ZIuwW~e*gEG))L|WF%bOxt%*qS*EA;`$uX@#+CE#TOEzD-2zG4q&Q=7cp1 zu3Tn7N$Oafv9cv>eo@8}ye9P|RRlYaI`2D!yC zYb5o_n`zFG+f$c^ERiKtFjZ>O-9Gd5M)>F%VCC2qftxNPe`7t{m?h$ez2s77t;<#x z!o=t;yJ){ARo~I-0#swD5(kGrj2=Jx_k;`9Jq)*a%?mtTA(2dT*uB|A9p))!Q^dS> z9+C8eBvuzaG7Tq~Mf94NBJpZ1^vZ7xqHixQg@5{9!6_2DLDRf!*V$xR>QM;J%)vd< z8WqgS`#1UPvy-*!udj!yEpD*N3vqG-by}0+s%E+={`O<1;Y1`VZmptaHG5d=V87TD@fZ$5hb`=*iigD|5khyiAO0#)Q9^Col@j{#u$dI8Edr8Z zU9=b))nWIBD1pQDZPV6r2PDVppQKut$^{*9z~1+IbGga>2LNQ*ntD2@ax8CZzX%fQ ze9Z0E;yaW6&?3{B!_J5cfp?DeK&yS)H=rEqIf1+%4Y##Nvy=k>(it1Icu)HFVPlTj zc{1xT$WqI0WuEWV8y$RbQ##y4_&RPOPbS`J9nX!?48b(>EkVCC*s}2JK^^dfRCg{B zY+`$tDV*R93jOzVmorEFY<3fj0KZ?&P!P$C(HHsP1YyIjAE+_k>ldnBS23@3u>o56 z8UdSS)WxG2y$im3iN*}$4ljk5pqd9ip;~udEGuApL6Y@icg4Yw8YUq zi6p>mo_^_W6o+Kk-t3*C=T+56&Vj=vX&Grj0a*6rIDj1Y1s-}?Tm9pYC_b)`QR7IY zFOydS!>HDQW&q*n^;x>6nWUpVvzOuc`@XPsP?JhfE;n}n1)B5Km0D$)+s!JOnINOS zV`W6E*Y8^wANq{76I1{jKAtiB%S5aM|AChz2_Qtbh=IzumU*d95cR zo%JJ5Fr-`#dPL;G*fxZM|DU}Bi)8lc6IlJO%+N6B83U+jH{KP}jBAU^{L@cl`kp_m za(TV_53wn+T#@xF>3}qGSG60<`-`oGT61BHYccw=$c5mdeKESZG~JP-XfyuKNDL>f zV5kvo<|eaRazZPcZeyaY*PR_`LZ0kE%m<_fp|)CRA0)-dZJq*%fERShE~oHkHqrL? zU4)f>G6cbb0mEM6xLmq&ETsAo9n{peo$eMDkry@MMdl*Db9}JR7Q|hrn7e?Ghr0Z+6;gPh>lCAO# z#=`&~&370V8%&AgR2(?eQ1JUrPvkDpZ0kjwbmBFM2;r@iCj*0b*-^SGy4BN{l)>Rr zbtG?KS*i_7fI(uTumPg)NL&UNtbJ+_?ajp=D%S-uFKi^|DnS3`fL-c@rutJmE!bl+ zxYbJs6#icYZ1VUl=Cc@!I}b)fHUL1D6f|3zHhGSlS|R;~_nln&wU-%3C)dkk{A2&u zqBwYP7KK3QRBUOhQEHb|tJp`5KWrKn^#f_u6Ylabc~-*Kz24Sfu>H|Btm_#H__m>G za-E8tWwy1XTLyTl8EI7hU-9KxP`zHi3NJt+Gd<4fzBsTQVp4^#kRSqDQ?sw9r9UCt zLU_`MM)R$JYiHYW&s&O~mw*c#j-}KI8mcU~%`M{lp);rfo+18!0i1sOn$5eQFPvHb z=Mq-U7!xa&&VSCZ@H?~{IkZJEkG>%kWilTgpM1Tp1d6l#UR|kY?;T*1R%!YITR^>wVHW`fNZS`I(K50OiZ>V8QBH>(ewoxXzZ@Eg&!36S>rnuzCz;9 zg9bXGXs-A!M-RZ;;`UGTB9(E&UNl_dI@e{WHyi`ka<`Hb7$8bV6f&s$+z~F0b$*89 z9CPCR(ok4-47HwOR$aB&Y~#G4lDhnv81iU2tr$Eqj_$V>%Q`$3Lf_fFCQE;Enf?5~ zNQKwS`A{TV#F%8?&`NW(qJP7aUU*x}d4)>dNN8|($1UQYXr{XUty&c?^c59W%(V|% zRYK;K!Q&8)Cd50+bi>xT6^TYPL~E(fjf6=wbYkS*?hECE5i|yrr$pz_D^xc-f_Y|9 z^oFL=^yXAp@sv=d5ZjtR5|e(iUjF}#eDq^hGDWDCzX}_~c*k13(BRSVBfXaQ((GI7 zpM%+dh72$Surg?EJiF;YCcW$}6XgAIBrh z%Nm4f2VIN~15?kR+Wo-w2#w>=+8EWl?u#A)7L4X>SwE&TS?)CkTkrH2K1>PulhpSG z&i`==VSj%aaikkCCCV&Jub*>%VVHd2+i>h)t+St4g_Br)byxXQMGU>?3(v_ut z5OPQzWRX-mw?*?_M8J)?Z$B?MooZg>9upNF?lXnl!-DrVsoZnxA*rP_ES*b4v1Bii zX!xnB3S~7pO(GpWG1GIsb=OzF6lpvvJ|TRrEBlN6NApN)F28LyUsLfq9{U@p@UE=4 zy!v#-;P_p0nSTI|h5S;9(3kuIALE<7ov<$_I&}u^OnD1v&O8!Upe;MeQR@9A*T4;0 zn0^|Geia>k;C#0ZuNG99u>^nNDa{J_RR;ZGcxKyp$}ZO5Kc2!(6z`ai|A)g~i4r;WS_L{yj*hIgt!4Ajdk+5y1&I8%y!)sc`bX3lw!8d_?7 zpsjVstfi*R6(e2aeNo^Xb2E=^D}tNQ$rqGAzQ_CMi)c8R^lMW@J{gvy3rd@l!|qq- z2rQE?XoAMv=E$gUqv&ioS+O$m;ff}&dzPe!3Y8oYYG|T0&eQ>}sN;Z#_xBFPMo-sh5kJ9Q5T`lZoO(@mS^>_Ol<;E-ZeBj+L<@r+>klm%QhL0B#GK2>c)TM~k}cEkVcvv(9$0O*ZvM}77(>e(Z< zDEy8nPddL~BR{g7`rD8IEjbJx?@D)rb_p`w0Nv;eXthz+)A>9CY(&(N2a)EE<=)ST zxMtDN`&f|67U~0Gqhy#imh#5uCm;@4?nuU(i6%MLaxrcjS(!-?FU(^o$3{z_(KNbT|(Nh{0b-_j;z7Xao|D zL`D3e$4xZAFXIFb$Tz})CB$@faAFFk_~kw#IzF81oKh-Vd~|-!Zy5i08w_^-N|Ep@ zGCfgpKf%HD`?IN3G~>XIZTAH3#0ti(Neg?5m#;aYe_cm+LjGjzaye@I7@8-DQvtqv zHkvyJ7gq&OI+{aaMZ0!B^xA~DnA6yd9#}l`e^#beKdlWLp6#ok6^(c-m%L86*9L|` zD=6#P`Dv%yn)oux_1guj@Cl5$BI4u6jr$Y~K&^Y+MuQ4>`m0^#RYTRuV*^CpXL=fG zR5#re0Y2b;BmRNT3Q(|HyTw#BcvSt=l%Z9*eI0p&83Cm^lBK)yvg+G;RYD5+C^}@b-d#g}Lbm(_tf3F$H{w~12^@K#%IZH{l6!}uM{$?~6S@Tq zdVO7{mfcURQw#=kQTC0zJ$kg~umC6nnR$z;SmU(+P2MEris!FqvnGC1bu`x6)XE5S zq&KK*GurEw`xq689O;Btl6|?jd_~~7nP}crm16~?V@>5!!>(fxiQM!OE!#)ePth*1 zG1SzMmYXP-+EZ$-@8>OCoMWjY64CRzOW6Iv^{YBkXLsg|Nc0F93ACqSAwYhq;vnStH(&Bb^Y-j8#Bz?6#w-e{k$#=MW*N{p1A8U+h<}L(pl*P9=(CgElx^ z8zG)L=UB?oqMKL%Amx7+Z~k&43Jdaq+vKv;lzBO4sO_xvxV1>#httjN>FsroFTxT) zEI?`^id1dT2#jln1q!!(nx&GzWiu$WtEA04#Rn(k#6`A?@xPCmXB;x}OV z-68S)|4SEo&pTa`XE(hC$d18iznJCzJHA&)mKi;yRinOuQZT^X>dJDUP-dilpXGRn zL8T~#_dHgaQ2?)Z^qK>l_7!I`<+(YVm6t>r{q4xMrIBN1lwg|nYGHbSylIgmV0U@9^D^of9^ zH>N7=UKC+Kf+G{p((&F2Yfl3kBHemS{8Yb4mU^ez`*9oQs(vqRkMfH!anl(J^Zkde zR%4cLHvdfG-cQ)F{2)@Bi1Ht&_xekRD-lr;@hS__h>L&OLLgaX8hrXhJ|MvpCtNk6 zyd{E2$Kml?9bw4`CFJ(n@Q`UZywmecR{?@eY!!a3uk2z~1cK%K`#IJKtX!HNP;q{u zl_pRJmI7h8Ynpih$eqNl$WABWzt)H^$^L0nOycSWPcoUfu$E_C0_$cnTv)0>7kL|% z8sjS@nm<%9pr(ye<8hMoQ!)A8$<|h{7IuXwdv!M(b4Q{q)&ESHLas!dYHHZ7s^g?I zPii_LQkMgEoej*j0`Nx;s+Be+fGzW%ZL}h8|Du>dTGw7O{EFrQXnRoKsPOxZ3+#zG6|;dMmWn1G-*vJL9Z(d0tNQOPFc`+e!%{L?>R(HU%DEpE zd%5yDbJ5DzcvD*iv}I5vmSX3xv+PglWv|BX2thTQMX!yi&vb;c($$Jf!wE)z%7uMb z(&9sj&m{&=Z|O096n+ZUcsGmpfjEK^aWIJhyzh5N2K`XlqCpp-QF$r1{Yee9w>TKK|RWxYUXP3g<3T&VSj3fnVlGRKW_=+sG37#sj0;m5LIwU|s=qr4KH4M92@1gRw7*4sA z*MT0W<3V7h$gNg^cZG%h{+3$qgVp*VW2l?xR+uDf|6RXpk4mXspiU==dI5l&_WIo|ILwy^1a&jRJO{yJjB37Zfz#>wZsXnC-1$&I zGv;4kKr9Zg(iiML^iRfQU*WDupIo78RcFOUim;^bU8AmrUzq($(m49#ZPOd1(9BNO zC&0*DCSIocd!c1`W!DrQ7Q_qp8w9_pxZfQ1M97x@GP{SdOLD|x*d!UP|96}IUu!9V zv3zeKV{ICFFmG7@If&IDhr*%}DaHbmS1FAB_F9#^pSUHX!#22johC5+KYJ!iVqmWI zOF~h|S8_xeVuQE5!Mc|XiN%gGv-6fkP<`*Y2CqC3+n{7df^3brn|d`FOqM;gPp7$| zt+u01_9vK>WF)5N4;SeD*n7d@_Ml0w(uTCNP*E(t)UCz_%X&)xYF169((1dSoa#Oj zS5&wIPd!u;{@rk$FMg>QYIMnM+z7zVt~5J1NkW}X@O_6Fu5_86%mDMB!!9=8gDJC{ zphfK_?L$sWjGr^Lz%S<64?|{Xh8YotKBg-6Gp{?C&s)ip(6Qdsc3C0~~xMLL~9lWRMBo&)*eY35ER+E&m zX$VYOv;m;)8BmtEHH5DSF+l5MGI^HJ_DuJQg`Y7scqsN?S_(INo;ub(j-8$9MS-_D z9`=jDkUoCEu0+B^2)bo0_#^gIRX;}2MBloP6C=+G`h`B_+K!O!Z|&;BaIt` zVI+%I06tE9*E@1Y?{aDoj%&4z>6#NrLA~9ieAq0>ipLr;41k_sw9=JRKM`OuwG-(#|56^`X zY%Ve$NUbM6d`ow;7Q77N=4)~Ijj1cpV)4;L*2r{WQJImdiaYu9kYaCn|LkyFKGFN-77*#!^# zPVx-BTh&5P2zq0T8%uP7zn)StDM9s(PW2S|Ng!N;Dhi3QysCr?vqt;C(v< zLveegyRYvB5%Nwj6<5c@!N4>@Y>V*iA9EUqL&6Dbbp-bTyn2Z<%)DHz9VI>~oimW! z(Eh#wVrodF$dsls^=dt8Y2?Ka5rV#k#$noAxqC2~H0X{dEz?*omWcu^Fgz2hl9{#; z2)5x*elipJmwEZ&+!1ghtDDF_3i(D#TUT$K_wsZy)jk(RB2CgZUz# zzwM$PkeXgAe<%b1G8d|^6L~Xq zPl`Vns7`qt&Siw9(}7&Cc(9uMb(?2=8u2QsH5x*+u=9i24nDku(W5~Cbl5feF=f^d z+Ej7PjKn<jcN=KogAwv{%*cCiqMqf~wfejaUxsG{qfSBCq;W7I--&Q$$cZ zFG49n7Ps#6vgD#+?8`>zW*$XaEmBJuUv2CKY}+wc`^e0f#a#@(+s?HC-}5lw^#mSk z*CR!Kw8A50H_IfQA|do0&dk8Gb19h1+PIgB5nC1`E)>H822@6K2U{;f$Dl%<(y= zaLpc-N4Pd~RUG6({@H*TKc3#S1Lw%g9cK@}oiG!E0SwU1sxfHq_p2wRSWQH>u$KWp zzPot1SVAy-NOgwon3TwWAov^PH1CuJ{sVUvut=WUa}u_uwKAAJ6#r_1VQ$*7+t}m7 zZoi{|kxXJTCJ{LhA(syAY$my8Gs5Pp{aDEaH${-@u8u>4IAS}Zvu1ZDU4#A`01?_< zrIwQUg$xn?J6|S}-I+9zMUG>fE_Z$cWkwZ-$d{Rs zedpQPiw>{jdjc4?(MYEvkRMJnX!idf_EZK_+M7Ny#3?vDLsBd7IY&r^Obno)(6_*S ztt&hqr)?=T>u`1AZRfIi^oiaWZzgX%6my|?8!xIY(`HpUEu}4)?JY5#Xyvl#+}8-9 zakM(CUx@;Xy9^a^KOtJGqb|fJ`|(z@mo*pqm#SEYG99=!ck2i zae3nq0Fi}#aWy4~me;ZAjn%!@8d`+pRaGij6NRQ7xi9}ChTAAYLn^|QE|1Gt9_=T| z+C0zX2I9vN&!!;!YQkgXqhVn@eEmIB{@9>!8*-Ben1ZU0rrN9gEAgvTmCmpktp4zZ z{m_*?c6wylwsGkCCpX1nxI~B-k^xxale$0Q7(WZx#X|R2_f7E(V=u)DrkTuN)^IY{F9fSWn%~ zRwr915@S_@Ul&36?&tC=CU(RDfO7rKmr#drK47kp`107@Q~TJE&^aVSc8;8X6NGIA zG_gFne}#`$tUf#JBbg41kKj)fjzBeSF*^%dIgcQjfDxOZN{<37E(lp5G7uvxMgolT z;UT`>;jcGqpk%3?u=&45rSWvxJW_lk;}7efp|}gJRn?9_?rw4&lfGGXndfEFqPwmX zpyA=rqYi}bxObt!k$(PrT3y&eB@h&NVb}0>k^{>hk zBP^oe3N5l82iM)>h)9|l1`y!a6$)ok>-Hd4 z;f$1I4X=^+Q-ILl@@Ja{4dJZuxp#| z+*79!xgW0nFCg!LcQj>6{h{xdtHpe03VJNONCY&D4Z*rCEXq+G|uWx^^od z(5O~x!UnX$?O{2`R*>zF8zI>B3~`qUV&!PlP;1hIp;m zl|U4KZb%_^IH*&whc1Zzcg_^{t-$S&yEwKz;oKj)KFYwMoRj9 zOa<=)LzG}-5v`r`XcR^#;*y8>a521~u{-F`(s9D=juwkC=gGb>e4M*Fpv|^&OZ%5Z^5Kok^!PR%bmZ z5V8JW65(H@5-0}R8iU5)H3}qEK<5O+*YjG{Ea55AK`5?Vkj-bBU|})Mmv`PcVDa4} zG^jMB-PH=FAex$m)r`5lxI7nMTSx#ICE&8JePy08eD-hZWkNI#`8wd3-`*PFa zD~gNE1a@s~KpsA6J=LzB0f36Gbx9-oj-rqBvP~t6)LV<+V6b^yvzB>tP3hCCGu67f z%2=y@9yoXs^b3$DBJHg;r@(5Yedhl<-G>80{#+Fo=mA zjDRV2=e8Mk@n1}s?=}IM=LKK~w8$-E@SP-~kU_%zzk<2rE+>%Ie^uoX5l7t7Ji`;B zLG5MTciel#wOvL8yA&Ycp^yT|R#kR|`b< zGV0zM;$>+-{rX2|*YA4@Q2v-(|~(;xJw7@W|S?x;gzC z7(*XcKLJULloX(E+LrbEWM=*n_!?uy{QExKL7j_i*iIy@H=BYya&I|N6bF}*+w=$n+OB0GU6}sOu!|yvTe_&wkopqC)tWCX|W95X%s2N%nsVe)ra-( zUtctT4-mQ4({i??6UWwa%p-@_X6W&_k^Nkf1`?0#W$d3x!#S zN2YMm!6&Mg<0VvwW}i&=skmKb9FMgkn_b|Hw#k0AYN4e{069R$zxMoFKfE%-CeofcMJRoplo%9Z}9waq!G^m|hzxDo1KWk-(lN%k;(edm*7WM z+d2KJl@Y_}eR_3m7`50+(deI{ZPfDTH$2C)Y_vp#rXm5Ssgmgw_&>TBAe*>5$VBK; zFOMFVv1iQf(&H_;U{1En_IVvG?_yWZ#p-_-j509a!Akx6*NNZR6w>Z%7$O2Q9%j(H z0nUS8zT-D|)J1uEpq>m(;v!SZ*sicTI7AI=(&HV#M^KPE3D$MSwX{P~^HyP1CRfD%nBC42Ee(T`1(9-9IQYvRfjG6$^4=*p!+!$zB&s0OX7U2>rW2 z=@c2s!NAT+S{R#!n-xMLf8>Gn^5Ga(HU!!bC?wJWmDW&z8{ZCA4BSgw$>PQ3b8vAj zg;DA1Cr-7%|I?m^6*yM?cz1+ILaONw&tasXx^!5GFxl(60#0c(O{i%yKqwP_in4d$JqCFd+Jk#aoyG0MXu-+ z@z2IT;OvAVqUjcJSukR{JsPgjsWDgv4OTD;=!GosZ{3@pNq>gAQ6O>nX_^*Y?=KHX=ZPTTX79ti^>X2!kPNn zEtG`o?%vNA2zrkOSn*QGZ&Q>{t?OYtwsb+(2X62kT-v}o1*pK_#)l72bPbL1(WI&& zd8yNrLzR2A<0E$a34IC#S2$*BP{@C@^S39MTm}SVyww8!)EWxr|5hDGObXwIP|9AI zTYI68O4*d*SVOW$df!p&aZKB}cv^r)Y%PH`s_tS?M#z(jchLE~mgQ-49hWsQrp=nv z$&<)WSg8L(2$O2kU8w!l`;vAW1~RMZU(?}~8wycH2(V6+U=YR_tOGXw4ZD5ceiC44 ztC1-VzDF#d&PlMI8X;OC-t{W6W=e*(h_}c#a3gO-x1gd}9CaP7$#a_yg4#b@dHFYf z7FYYPXxP!9py+JEp55yYo}Xwt*0l##ib(7VLG;&!fs1yj$P}YHBWIDP}0ko>2 zmW3T>8H%XB$OKIO3ME?9>QBBpFpIGEWjK=!FZqyK78Lc4qD<77zUPm@t~M*NjC_m`tKH%h8)(IvWSk5 zzT{p5FNspH5*~D;@h{||{z^SqC4cB>BAO#(C%vb@_h{`^VP0)Gz2##x9p9#3`I>W} zXU4(oD=!k!a0~RGVSpS`C~dU)=aCoLEkkhs*T~Q4c%Bo{*&O}7QE_0TX|ffx4&2g7 zI<6tFL3c<=LPE>WR*rne=;tk~sVq6XcVE|wpwAJFRZ-WEWJ#Tel7S}sZ z_g*)kA@`hk;pv$g&9)kjw~Oh)%ap=FE#vzw8*q7Bu%6YUR#hq<#V|9O+{6f9)FM)F z@YLQ5UiRX~VxZlkDaO8VFo<`52@S(B9vdzf%(1kacOKrHU%^W?W&k0~U46Z#J!DF- zqx7vtbIqckkV?k?Il(xJjiJp z-&OK~3>e(xfw#VZ?!N4DoLdQljlz>&hN$kp#I>;6NB1>Y@F z7-z?$9aZAd%dy_+7vun<$}&#ppZ%sqn!@z!X@7L*pMaE0gp~K_}HtO%T)#coy zn|-0E%S9Vb(0pf?`1Q)rWCVS4W2_?|Wx3QF@=On5_Q_PADo^E!QU}pu_{dzm!73S- zCK*15^?0STkD>Q}h6>P9U?S64&W;6X&ByEkTsoW@f@eaog=_yNmN41AEMRLN%vFzVS-Yze~jScEFakvhU$5c*hLpCx0D<9LSB%o(}Iz9Vcv*yd=_s}CFj<`UrG-! zQ8?9e$$Z0pR4s{o%tEej>5Aaz2p$RvmM3`=E^ZhiW>n+URngDP^9w|0;RzgPcWR#e z2O}^u)%6FoIH?uWn@j)%LehO6{JL=jd#WoKcWTs6rRFv?XlbjZN=^mnHQDj11bOhG zTSYPA&7?;2b}a5n>s0chGGCg533$gKj~*MCRI~pvTjjuiMbcRilK+_q5lNfti^2{+Yri)Wl?{-(|<5bB`{U0YjAFQ z?CP=k2Isu=7gpGq`nrZBA=D>kHcR(u<_BHd&w{}Dw}k9Nt*E1ERt#|nsReyiFFS^hSqz&cM2Rz02Hk7^n zMWQXAl4bb1SS>&c_%IaMM*l{$o3o(e_mt#gs3}}kxW9NJio&a^>drA~0W?HH+s-f~ zMff9bDGMLSxPh7UpdhAaepIVZbgg>$RH1zmh(e_LozP~-RbnexnyJ!%IVDOso2T50xh)@?)VTO9&O|*@By$T&1bb?j^j|l z*L?L4^89^S)Ou2FK#g}aIUBNI51N67M2ospGdw4k+ZYiHb;OT<)4Y89>g74aUh8Oj z5x+M$jx;C0SAb4gQ99{(1{WFMO;Ok#!W)UR`^HE^%UJB*?Y_~~D=GfT90JwP0uM)U zS_Q(t$%&UNa$Z@^rNREOw5l5RfhC(+9vtl*cs;JMkfFROVRy9TP;EUypOh|3cwWZ% zO(=HAEfRDxs&q#_R>_8hq{DXoiv7Njjn*_TRF z?nYk8mF6MjKh{;IR)3{{)_3znBV9<`A;GYM*#Zbv+7cKtmf(k-5g5`b82N&%4Gmo= zk?L0h=yyXIO67byrgmr*SD1kiaa{@yrd}a$uOarxD}4XwcgstRnqlU)aq7YX$+9hJ zc+(3eo-g&Uucd*%Lx?dPL)L@?7@~ifdqY4jr6zXcXg_h>o|^z!`;3z`3MC#XS$tPx zowR;|38E_9ZyHKxNLT<7LJ!@i+Pfe_TW{2qW(X+KJr0>_RL*LTk^zF{i|54b%_?usDeCoE`aP@7t^&#h=B}m(3jr83ElFGkqU|u>Du%9%~oD zdh>ibbAPw1t`5u&gTV8r9~LXEIR|UZ#NSp+?2b}2nunqI6v@-arI=RhO)wP|`-SAp z$g$gAvgd?}ElglFFSTLAoHvU6)){VZ9p!Di5ivh_G?;D(e~VD$wmnTW@XXylM9=;Li5^EOrU%&JrXs@4&COKGrLr4+g@=)f$wj4vQFkM2w7h8oMmZmYp>2W)1ZNiD`5yZJJtnAt z@ui&G`wH4>K@Cj+@}RU1hrR!qR4t|31g|}&V!K-ZhU%Jb#`zg0 z`$pfrkLN#vE0{FMgoTu?&?*JY-)YD(4XZ z8#s{*u#WvO)bj5NM$qPV|AaB<@rIRqm6q$lQ0v#poHRgCA$?-*Dk>ibX6GfmK&{k3xo0L>Qc4i0j zC?PjSib6r_NHWsasaJzn2dZl7_sH*ogCy_gU?prf$YOgv_mpV6P6viyC6R~d(>U!^ zpT$#1oX}5orla3h{pnmfU%!V#l#s}x<7p^UK~0wkOVl!B4wWgBoC@NqwxPU_oLusV z3J#+GnI+j1ygWRgCoY_hB2X?Hla*|e?x6SGf%3Rk77ar@(J9LxW=L}KCS}DbIyj5W zBJ@Gecjr%>h2=c6I>UGyvXM!e0?btRhon~`+lGqyeG3Z6+NzB5<)&P2oRi>5Z?S;; zV8gt;hxDEEJ77Zj(gozzCItP|H!VNUUaJc=6$u(!?GvI@Lrw>D+P#h)LF+4j1qDB* zOOeu#$oIdRv{`CWFUr5`82~EtNId%tG9WN2oA4wujt_KTJuOW4@_dX`IULI~zhmgT zak^5sJ|fo?-FE@*$RmICUT=L?fOZA(niWYONHS5~tf8fdk3hOmLyYWg!PPJrnET}H z(BQoprV@5%tS`jc@Z2=LG(!@`-YTf7W!d_2ZSA{w9_v6RJzQ@6)J9QI*1M^rG5$vl z?0%}|IS$!`l8Hdmt{<%jhUepb=)r4_k)Kz{UxXp}!7K|uK>+&YB2lYmkS*v{7@ptSqyI9zO@n4HR~~}g-46HO#zi3 z`=b9cHuFLjCu6hJ2)uFeBZwFBT~7Ia*j5BWsiy`I%-UVjcmKJ+D_C{MeaMqb-H75a zla9v^-?R}zNX0ff9qL@?0Zqt`G}?m(Pu$nG4fVtvynMXl($l6#B_>@FyTRXcBQir* zJDa1NemR#aIwD@gskl18`2nx=+fE{mi%U(XMEwwb9z)IAQ69&+!1a1;v*sp^P4y(E z@>enx>U;?^KC|7Q^P2hEi6}APi!<*b|N4}^a)j%8yQuT1wgs{j`Y@tp-q~H;dY_Na z`t>4+6Fz!IJ%H;)iKDu~l_z$Ux8tOjXLO7)S*pP1Qmdsp`B7IOXne=}VG!4lpz}zl z*-@2=Ou4&U@VR&06r{61LjDH}MkcT_U;;nt=ZU@m=tDR6dIl##1yF9f|Ay?krRr`? zOAy8rbl&cx-Peh=fhWpIYOVeKSA{PrznSM18;gS0bJ*Pmw>}jqe3CSb=^?FF^fv`l zJ^e;TGms#Q@N-@v85VIfU<*6KuSl_dgO`Qz_X$ZUPEI8G4JyGk)5n}U^ql-Bq3kYR z!y2^UyITBONAgq*o|nO$*V6zMx%7#pw$05fQ0M<)r-FjD_c(6TK@FLR_DkVq+rqm{ zOqs=I^KH1OWAdzApI}RsN!PK?3NOW#ap=q7_s~)qwl+Y`;{mdA%P|z=DQ&%jBK5b7 zzNc{q>qVHPu7W@3i?jM*60NMOF_6CWJ9>c!740BvFsG#Ir*S+w}9tikzMv8%Q2xu$LHzh;ne|eH+~DG0ogZIHPOPT z{_(RW%!xJ|-ACQ)&)#xX37>-u-jM@ubEm=SxY%*0+K{A>dUm~~va zSn;RcreIQ1doh2dVyJOybsb5R8rp{!O0F!StdAA^Ps(~4f_X)VCSLw~<*u{*Ub?1r z9hX}EC;@7o9Q1I?1tdj4T_7E|aC?+*#Vdyr3VJ)0|AwWBz1&n9NQ)=e_WF(6$cbye z_$QOPdwD+uV!{}o_T@54-|z6};b4x+B~GJ#K(*>O+-*$!0nHi2%*!n6-$RB#qh-5R zhES$b)jaHzj``^oL#UhP3}VF{wt5|5Pumu3me;=P0v#jddG#zfzrVxuMeJu zX_?`U`osT%nc+ydzB`W(5zvNGYYz5X$4Vqj+Tc|Jp~O@~@OO;6sVg;FKmZ;3@tCWu$fK9+P9hmUaYT6N6TK6H9h|7w!ZM2I2c z?P2qCwTTh~42`65r(fY&!mkql)ArpP=;e)QlnTC1&xoe2~ELb9`WlAD~3kJVpQmOM7DX2qCB`Pa}SGS5;0j`P)${DsFy! zJmIN8U^k$xs2aV)T3wz9ScDDe^#{g4o;FhSU~nl1CG0(qAX!j@f*|#W=|2dO1-E#R zT0ut|%tkJ+vuKHldO6g?nhi|Z5okfsHsfb{ZARb7euT--T%wTF*(&slCr&EEZ#+Hw z7Zd``8TL43iVN~B>!pdQT{WUuEIJWn`agc>7-yUBkN{oD$>~Xj6)l}Zy_$qS-ZfbM3E0z2m zIH%EWK03)8LWr$PzmW`&n4c9+| z5pVL`<*}sagOKB4deZw(aTIH&My~noCoezLn5mi(0M8N>&CzAmSml0LKut8fX9xvc zaZ=SqJr4nF@7k2=U09sAwKC6H!KVeJc~F)B4xuyCP)^l*s(v7#V|z-!DcJ_qn@Mtf z!O-^43<|MS>YEd=+oS|DwsGNNyC@DW&CoD>M#ibw72K&dpXfp4$)enWC*$})QKb7a z(aXc~;H%D}JNg5%(*;~r*_4Ix*iJAiW zflE`+>2VT4;9~#AZ`m`A>O<7VWYiS4Bs0F&$XBA*3Ak+)Yt9V!;b?Rvdm&E$#XNKVh_APsvdyc-?gcUvf>c-SpXq;w zs1_UQId=+hKH}g~a@zn2dqNG{0L<-#OGT_mQL|#Eg43Q*`zy9O{#(Ji@hvl=%bAai z;+BP8S%GT(6bhSE8+9xh4*f<62lw}7oeX%!2vbKXWg}oVlK|k|-4=+vK`u zeW%r<%Nmi-(mT*X>Pm%FW5q~mDf?r^j2*<8S6*(gih>~=O~2}#D3N}>Yj7cN)vw%g z>8j%MzTTuYE!_MEkUJ~gz=sp)eq%W_#R+B0Y}|5+*~`0*{}5D%fs7^o1E!`UnG6b> zVq<*jeDgBSD0_uPeNUknT`{t`_^SEli7*W7r;&X`((DUYXJ*k8+jAcT&W)wl(#2N+ zcnHlakuNZMUdAi6`cQSAD^)`F&ZtQ8m#egYq|t zd4dzdp_YLvmy$%BMkGwd&dXX_bvfleS_uL?h6h$l7FV$#>MvRDQDu_O2JwXg+g-kH znqbhMl)>(4u)Wc>_x()$MjlauNkMSqAngHujE@x06^T>G07`sf?UI=WAcolT@F8>d z17y%|G^iF6gGHXZhX9*MmnOjWyh^~3YqR1euiA!;9e)F{A?@76MV~B)*_AFm_JG-z z*k4EFy+x%Ue$Hn3rD=RkI(f`Cahq5n>7b3?xKQy^3?BX3)P*AS+o(z_Pq6|8@)j#_ zuZ&J-$mX{S>$WLj+{}*C?CYn}X$<@1olm~S6fiUixx^S0;Z*c}o#N)C<*JC9r!Hlx zXzQ5+&2eM&G!FGL>BX0OHi65f0-=NGE;A_h-dYz%WKd*V-$)!Wia$y^Y|IZ(g_}Ew zVN5AQ^o<`l&}^VBGbaLXNw?p@wE1|E&oJ_n+kwRK0GKZOc1%-idbJS5iYe3GBj>MXLlA8391{2@TCB47@jVR{888e?PW$OTYuQO-af_my*N?$BZPkCthGQU+BV?+SsR^Tvv>Vlgp1$WXHSPJ!oSqtIZ z4(r@F%(Rq`{PAljMI2>M>ql&0%QzHOcVM*R@94C*5dQ%sapP-$LUm)&7gTK~cEUB3 zzw$KsnxVVE%J9(fMNsan4Nvawmj}2jqc$AP%7tq1KqXj=&&xV^v zYBDsL35<19dBsaGm=1AY1gS-pDTRUW!20F_^kGl*?eUDDj*x(|KyywoAlaq&lLS#Q#7jL`>3JSqI;{H}H)+@cXawv=%paqS)>QD|bQ1^7jPb8ZMK(_k%9B4wjg0IC z4W7;rZzYoQXNTDYP@*MQI&z$99uVw+*H5WUzYI-PeFD)OrKNK#Ogcdyj@;VhOkTO2 zz}H6}wOTYIsEgb4=(VAM-;eN!Ez^+R=Q|UbNqhzMK8eH)$+j#xCK?gC%Af)UU7WnJ z^X=i4H<(YP_}a#xr!HTn{N!y2T#{TeuqS2c8M%u*11e>+Rhi9}XA6vOP1g9xRHBFp zNp^lir0SkLbfk=$-8iH{yjw=R&p`#ZV@73Wa=J;Y@@9tR65hv@oNI!(23&SlW$vM6 zG$rp|A0OwZoqn8+2)Nph`^^WXaF6NjumONh&JoS4D+Gi>xXN z%$Kd^E$EaLP6X3UFI6Ovt>fjD;jr>Xu4#LhIrUi-d9^)Hu5_7}v1+e7+d7&P#)y-^ z$lowG_nlG#51_KoN)cs>wTbNFc1Cwxo>%N!|M@K)x@2UaoK21NGtB2eXbHxLoLEB2 zhUjcx9_eVca7E|_F1IH@>+4peh>dRd7#oy2rx8L=184Q+5O`9&h5*nM}))T(GQeb|vEJeOO zn{qsNem~H2zdl}L#I1r|3VtdRuleZtg+1t}5eWC1e8O&sXO6=olR40i?Ef+1M^F=P z`%aK1*83Emt})OjrN2j5w`E;*3Q;{dwn_u# z+vg{^{=E9k%hXIrW{fG3E-Lb)W=w!{DKWOiSd=c3G1=QRRY(KknATqK^|drGVL-%U znt~doXwHhMWxP&n9G^|;Kia;>OfKMKf4D$f*!{oA{i;45<76Uo@C-W}E_N}B1q&i% z!AxTNQ405^{CTG!5V+O~a|~g(M-ue;V!Ifa&`X+DjujVM9f-me{iJ$JCVSb+4%Qvp z*}Ib20@GNF9ienNQeXzWc^pT3YX7*SBG?sN*V^MU8yKeW?|RQdD?v|ta<;p@sw>?Q z!OjCwhb@~{MFzKj;78%0XpvA2JAVoq{?RQVzOocBGh)2B*D?K>S!iz=l(OFaBu&g9>8ON({?J>F@eP*N&Rd#G^jV z`n1_9=*aH>pI$O2AT55tz36=1yCRiNc0r+pE1n|W$^*p@-NJ*YC*F7Z(_9bwnvsP7 zvHYEuS2c){(vp80iuLNomk;u2+DL~r$<9K&=wa?F`4-|btJo>VcD*xsvQfvyAP1Ve zFJ;JI3Lcu(n$s~QDY1dFHuNRmsPXDE@kj&8`D5T1vcoA6U1-dW>dWj;V$e6y=&+XI z2XiLCSzGz56OAK?1UaH;al{tL7hxdn3y|{YB>Kc_EDnu4G}Bt>qecbb7?j*5ow3~(=nE<#v` zQxuj=*6_FXkP(|CUsk6R-bb7mqqozJQG;?4)%|(&foeI>FV&BqVOI7iZqOBcAr)VE zkpn=Y{pvrb`(ro^iZkRKFYkO)+*lTSQS@Ow?q;}$(Q`21+mwyCV>@79Z?zpPv& z`QiEjgQTM(+6GrXVMP-+1}$x1-w^gFEW4Q4{rqO6EF41F8=b*?sasE>@i|6-#1wr? zqn#&> z%h*-Ut94Gs9!x$023sZXV4}Bib58r;pg=pa^yM7Z5|WjVSbsE!ThX_#FR{~$2(@VC3bIgDO>NNm0VcDiNgyg!}p9qi#OV&A?A>842 z{fRsA65E&TAuN96zMr-Fw|~4&i2I#O2d@0h(SZ*AnDW?U+?jNLL;nqnQ~Ho$s+UZ_ zuoQ1QKs1{GHlmhf)>ar2*l6lg+$d5!Tk{727KswAv&fagP%=664smT%TO~c&!#$&y zT(g_98+mHkg_^Ybro(EC@V0gwbcBv%?-m=v1W!Two{)g!|6nfZ7!3cZC>q(0ftFx;okkCcTf~oy=|Z6G49RKZoqq`YOYlL zM#T$NBOA8ny%di5{c(-zXM3y(wIAik9_=nAl~cbZ~#k;Coq9L0&M@O$k{9> z-BAZyZS8`aX%~s5HV%J=MTBSYx#i7!c&~{jt`ezTg?^gQK z1Pe0cl3Jy#Rg_xaycUH-gqJ^?0U=Y8c}rvJvM30r4Ml~AulfbElifr=azuN}OjCY! zwj)X@BK*&!s5|Bd%H%ryZ2d83?iu)NA>L?Tq6CZSf50-<{8lFHe@^&K?40vx?Cw5m z{N?H`4kiTrN!$MVz4_min4gFVQ5D-fj4s{$(Qx*$C#U?bpvPLtYkI8sc&eV}X0oiE zV~_vOFalJ8z}J3)mfo21?wPn?Q^I!=itce);UqS9RY^+SuWHxwezyb!ck%AeI8GzQRcg8Da+PqI43(&{GKE~JVRuJ+J_ia1iJQu`Wi?j&lyB#G( zCLQ$$;-BW*LL)A;~%apIEoe&NMQ=na{L}zLxq&!wBPz)*- zks0U)h3r$l)m@n(e?W*dsr9HZs$3-t5)zQ!*D?ekmpqeqK&iOjD-L4mlg(%27*SXN z*86)>Upnev$B|}alv*v(TC)pqksx+vO7e;WwS9B z(W}#m=U)0ZS@ZR;07?{=6^m0=zR%l#v2^p&bt7^!jf!x#0eabt(o!Ywp0kq7t=3ZX zZ!>hPS}`EHt<@~Kyoe+LG(Y@N93pXdgu*_B{rH146EQG_zW8k7M=x5#8A~6&fi)$P z2qewFkfm>Ml*dC4aXD$)5eT4)&s3)2e1^+5M4z%5>)u25mDS5Opn+n$=+0ltE8bP1 zTyy?MJ*mEJ+k1PL17VEhd`++c3wNXVtzWtQx2h7{;GhyFN?i@CMG$~%7_q1<9I zH|X4Pq<^5XmkwZaR0{M4bJBvPaOuin5LOGPoReT~a%pRvDI;iViZ=9MuMM(Dfw55${?S`MZ%AGq*H@1zcIf zW*_Nq?WOJWh$G#fTBS`(hiy+rvm468zeL z00YCtA+A^@MNj-)1c*q8Mgi3c_SjRq!?T07I=4-uLohI(#R(BHv>?qDj9d`!We*OQ zskL&Hy*s5l)lq;?k8#@!;3n$#RNBvuc;0hkNJfi2{>nXNa;&YJ$7YiV%%y)dLjm4_>5GO zv*Z!r(o2ITQ=JZE;iVQhS3Pt^L4B&CC*q1me7V&HPjK0u^i2g6T-g<0EXj10Dwl~{ zTG!ynY626r2NIJfavueeyvH{?rI{Mn`G@WDXroyF4mBAEoC?I-!(OeUfIEF0@{%uG z=9&2kROBX^wnJZkk^Y4To*vSaQ=lg3FwEf;cIW57HfrERaE$fXY*rT2gX3qKB~@Qu zU;P%A(DnRW`SE$LX=JHnzF3OrE=Sxxrb6A13KfCGe9m<29)AASnF95;iLEV%jn4dRz#^&ms@1`z2xye#m@&SP{ZCWvql0IGE z^0oD36ba2*U zh(ZDCMyPadpP0_m8g}8Uszti}rAoBu7VNjb#=Igtf z6N$#&bnGgzbb2-g63!XTpv7E*PG?p$IPtS2#xoBhOWADU!b&EqW4HTs{e@d!ZWhgZjKDYJx2BwQ%@!jBH!WDR=QWt(eiKVe)xjIKd~ABRb{r z(@6p|E&Ne?QC&gzsRWz`T4v8P>RpHjIqf2wrj^3mN!s{1`(8*1uq;sgX_k-|BiiKMa9X_;wa>cW9Id%W~`eJ`Opn$z0(IZdN+f7 zP`m@#H$GRRLX- zK6_Z|!v_C|bU7(BV`pHhWae@Ep1`lFEO~2$YCIWkbX$|y;DL?7Qki`tL00ja6aw9F zrNe*1s@-JiP|R$dekqeJc)G|9k?=c?!GN)P&a88f?l<|J{Fs+e01i6xGWe!j;+r93 zeb)DmDJH(UuS&ouqX`Ldo}Tc}d_#*t3eYX=Zi-%`=Oauf>R;TnQdcQ&^Aw`!zAp*g z9Z~!RIMetVWc?;mYy-}jn55=nzDuDchb__JT!mGeIwKJZ6ZmQNPfcHdgtv#@D6z=E z+*}>MaUS~&y@$W{Bl6ciGWGLoNYcBebgP&vB_*Mw)bZ6pb|eO|zv0W6r$xwN8x7N> zolbKTeA3ZRa)q2TxfAR@Cu9-&?^U9Zw|40IlMU67ZTsGivDo@={lcK(67gni6%80k zHh}5v%C(99rTTZH>+{U~Ga7n&E6_TEX=jOgeImhAi~Mo5<~Z^_mRhMfD(S#4Jz|s5 zNu5_gIbD|12rjV;Kd(03FkP79)K8z&OpQKU2+-iqRP9Hx80!nU*;|vE2l=22U53p~ z-r^B=GUNW%s|u{|S)bWbnsn)df9md5_UfyS*pjW!|nmKZ;d#T~YEP|i_Mog@NJX3?=^-*9r- zpL#3B0zZa6rYAAG4oe^HE{`P3VhBe|zEwa^fC8>SVp8^)Fb#3;nPeF>TrFa(}IxGEI6O8ito zHJ_r2gV5J?mzmP+2l&d4l)nI7lc3RJ(^V7UtDZ1Jn0 zkX8v^&A%4=1td|U28Tt;4C`w}UjxI#(QK{37@qb63ko2?)&n4 z!TFt)I})}-=j6|`zWb%~;@*63Vxj--Y?z@TXmy%)m<=I=V~hy$z~m3`E~!pPQ{@|L z3s3Fo37>U3V|!hy6ra3nDSJ0bO9=Vk=pqmAXQwDZq(&h?oq1oh83fvWJNPDe_Pe%M z8RsMG#-)XOKy!lrPE*#Bb`!7I^j)g_qUX{I;lzYdYrPy?q#ovm)_Ixf;IGEs$<5&_ zhW0GCgPsHNhQ<&ht+#0+uPu4$N+cW@C^qk_esTx3P1=-J3q%w^#G5$@%bUwJy_Ctq zYYWM8c=Vl{wh(0gOwXs4NgLlaAkCDab*Sj@E$4n2oc-({J^CKOSHbn@$Gc7wg^RAH z$k<;G!Mrz!BeWCQQz4qN(HyMProAe`_!|Rf z_>=RV-J8%{0!NY`=X;MWFd6Ll_H!$(+l*N~78R3=%(5m?unpfKNHRlYy#&LPK(OnN zdVe43-gK~DkaMy!Xt%p$6IPsscS#BL(gfjeU+_rT4fE8LEYFJje;YH6pAeNV=~7G@ zEHAnt97ET=WJ7a$s$^i~1m;O0_6PVD{Z0 z`eEoIanug=RqMD67+zXoO=UE?E{GS<->DxPeiZc_#Wv^9$#G^QVh&QRJFhJODbr$Ewytrqvgm?kaIN~RV z1DgEV_1jlr@y?{&FWtMdqLstBS~0{pjgNL$YVn;3X=B`wnW*w)$b4Bg7d;r2Kv0GK zO@J4?&>js*8Zq(`g?OFtne{zMce2Y_0KsVOkY6L(!^82-(`NCIoiCqSO%Pen1p@E$ z+y)HoTL7Fput}yZ{v+mRr~Bton(wgVZiXf&n7fwh$&mO5%4=sHd;1lK|Au(zKgUB) z-M}^4VIbu>-4};~MBR?51=+b7R^i{UHm*xPAh`u%ZEIKo|L;7YFQ- z*Zh2mgiXD?-)iG=2l8Lmd!*jg?akt9_6n%1SiClVK>_~gOTJ9&ko$X(JG%f?3zv~< z6)xy;NMoH@^{$7BU)be; zGpoloLlh4clDazwvt2;zWjq_sG!dgQG1_c1cxUD49!fN{v35@s1kA3buj20}4@W4A^XokFnX9O56ILt}`o{vm`$ zBNYCv%sB6C+`f9Q=kJ&uS0o6YGgt)S0wS4CJ?X-P)Gdm1D6g?cjdZh(6^{0wLqL0i1d2aV zumyNUjJiR-QIHJR;lYO6s?#}@r|mtr^Zk@cCLUE`c>l;0^1($b`Y#0(1G>2qzerD5 zd$R1-uqHBr`(PR_jasJ0G$@1;-NvsFk<|MEyy%MD4-N6v&#U`qJLaTKRpg^`9}Dnh zUL5{BsM3gip65~rMpUB8(e|laAN5cTkDXI<%;R>`uP}5JUz~VlxO83gnIFm`!Qq)_ z)?&#lAjAbTxtDi3X8C2f;jY~ZM3Ct^i#JC};|Gh~eMN1HpF8@v67qu$ zcp`Fow>x2)H)D+sf>DEpwJ*o;RiCqg$)~5{b$O#beu%RNxE`@<1;DVv{^1-e0>(mI z{SxVyQ@8$m#Y0WkMR zsM5y}?U^4!!a|$eL+Mjp(vAr*%U4=_h1*>ZVh3%ni0Ie5baS|GTX4F}yTY&l;GnFA zMqJ+=DTV-V7bNE`fxh~_;ecZa{88RqG;ABy`K09(^&1!PUc2)JS)#6An#Gyn#{(<{ zL7;eCH89-Ufl77_ZP{40u|f5=rL8<$(XMRy% zx%_RJ&fKRs5YI5 z6=dx#c$3<*9%g%aHoP@gU8@Z}0-r3$O~IvaAvS;;ugwCJ!uBq11<8FT&B)@?s5Qu4 zhcKReMLlSk7do)_SRo)C)j;tSCDyj5Fs$H546G?VKa94!MW6+_a~TJ_ z)ltQh@P+zN8g#Vmp}u%GoW?&3N`h8Z9*2)qN8^_&^XG1xV|uyw;+OzZ8&K3aqz`UsKQA-2_Z{C2#j{Ei@4{iU~jqyv$ zYfo5aA3;L*+>=;niSG}@e|LE-Y3kYu7ej5B_a!a-0l}|O41_0A!7{_ryw}!g^80*` z%On0oB*qrvw~Pr+gI1$e$B zp6$u3yb3h{Mm@D4o`XfCC}p_*l_@MO=&66&XL8U&I10RUy2CD{ZGuJ!yf-!+m?1eQ zue?LeR#v=I_J{1k{Zgr3|ME`rgDVYheku-Y|9+hd8b)p*2NR*HFcre4jM2IFtk8{D z_J=^IAw*a=^L3_~tQ66KtP*698uvFhmTW)!=gDNA*kL6~wW=o4gO})dQtZQRbZ5&@Z2$eh=2ay$81J0h zQ;6|n>&vDt^0YQ<+01lOiONqM1S+}N9<*9lzHT*wtNi7o^+tJX6y$+c*Vg(`Cus_l ztOrre#)9=fpMcdpR@Dn+Yz(nt8@muN$-PE7b_{Ef`f!XPv@Up+t`NM$$2h)U3@@4X znS?@}LbFmCjo&P&$$~Fw(~jvRcS1C?V}WlO9bG1HeXC*mb*SB|3$n!wO{XXPOW2uS z6B0q&?<9Q%pPoR{>h}g(8eXtEN(3O!>4)g`2l7YC)3Yd7qdlcA=YR{Y92KM+8@|oc zIjKcM;`iOUIYlu@XooDZ@}!V5MZ<%3$&mtf?kjW3M*DN=Pbb=c zs;a1OF7=?&5Z1^ofNbKj2E**&wp)NHLs8K2LWTzOO5lSj*Nae>;MW5|y&`(Pl;pf1 zt@q|}P6P|+AlSxC)9X)_7SlMT2P0nl_yHPCq9)qyyKjPsCYY8duM>ow{pkF-cI#qt zj5~V7QW)XEF5_XT_gtIviDJ`~d-`!P8%GGNUWJ@!Sm*J0q}1o2ONTjcgzg)m3z_o9 z1SFOph=fFs-BUhfPdUyLB`NzE7rU)AyMLv#+Ur8J)j_PPDprv+yK)`sR-9Dl0>}V6 zK*YbEEGQZ!_l5Oh!3OH7W3UR{Juace31OJIHw{kmrN5Y8q-P;OYyC z*GZS~a`0vvfj0i(Pn7MJN443DixZ?^HHrM4h&8mIqA-oJfc^!c&l>{rJzzC{-|ugr zl*_t$B(%W_zYOJCXJe0zFn%%TqBSE!nPHDVUS@Vl)}HR2=tgzy2Zb^>X=X;~5s919 z*B%W$*l&c!VEMP2%{HPHa-q|FlvUUY10aUE0;3(WolG>mT<*ZI%rA$ENB~|VF=Q_! zMP<@Zq6g?_3*)(EIu`A~5oPrNgK5*kt76%MU7j4S1+?Dy#fJrM7Y7M0zH(VeF&=$a zfqTlI4KH}X=OZL7g`9?`vRqA}5i^y7@D4Lf(g;Ik*Od&Nz$e?H07g65>+GK^k%LZP zUrZJrS!q-l7HB~J(GpEEd0gfDPXs8qK#8Iyv|yWlZRS>ZIKe}p3aG|T&A=j{sykXM z476CG-XJG;=uty-!N0)O;|-1$o9Ni{WcbAgHYyd#d|*=-twE%hZp*1(fgaJ$+Y(ai z_ym?zP?>Of@1t`jBUB;Jog^Ezg*ANuBe+DC1lp3rU_b;0Ia%V)E*izH&uHc!2R2&z z1Y|l$MC>Pbt;vP4(T5k)(e8(I(F?eKA)IeG@Iv|;vivL1N^{?v|K6)1%yDm9SDcKp z@t_IDc|au*<5(ZdkD?=J^pZg8o>8l*=N2T*(JmQQB2~O`*ax%d@di~s`hvbiuP*p(*1OoCl# zr_yo$`{FQ*{u1-uKBA!~Ky9JLs4W`mY0e|kmLGt$9%G`VZB!|(ZOKS!`kjW@P~s*5 z1L}8|-R}p33_z=lfbeyFXBD=Fmcm1MG%ivY%gz22!oKyWR#v@=jcMmlM4fR*z%xR3Pj({Ov;y-lSoqh)K}G*w}9B*##yd+x=-gZgQkrH5+g zo(f!QKFKaU)9E|)PCP>{^0skNL&t;`)x;0SH39FRW&_&}l~wub65rP*ES*IpZ9J&D znxA5}0;l{}VtB=SX1X@90+wkj>K=^4Ja4pjJ2Jeye$-&VEnf8)j4s9HCo(aKr!ja= zFz+f77$P-gO0GE~4Xnbt4C~ijsOxUncDpD?<(yvzTZP`Dx>=3qIr(=b;N*Gox`|R@ zhRwDgcy(OuT3vT&)kD6hU2YlwRbnB?HWv0iqAYdvZaj3GKORq-7U_X?YheQN9J ze9tZGZhL`VytC~0cde*T9ys5rP$jQ0CW&5pTglSdJ`cr@gc`qDp!SJvsd9|ler^di zG2v{@#OmC>Q>@wmn% zBmF9=r~LB{yG8%MZc_g21kv+J*xgYlo#C-RK_4GFWD6c2XD^Bi5&<1evLAfHo>Yto zGq(`}7T7HcA&2KORPkmU8otCb=U&`!WbLF_r_ij>tidQOoT-T;M?}l>X7@TkMI7r` zO|SKjtIenFt6exP7c9NMT?!aB$z#S=b2lx_#@tCA zmQ47d{Q@LE5)GCgIgP)LpT?o2bEHG7x=-zI;v~jKEvYzm;O~xD0;Q*pzLE&~h6@@b zFqZNL@rP89vK)z(Hs+w=Pf5Ze2}KaEg3H#Cc9k`JJ>$uVWkTwwH1E{@@4(;;Qd_n+ z#e`c#SB8f`eb~_F2~It;DP&DZ$!oM~3ZQ!7W17W;DplsY8)ILIRMw?u{4y!^?V@Rt za8gDZe5za9$raA+8Qpk*Z>l&0=8&fOb@!=jUN?0}j;ztw%$rlaCCvSDpiQP!SCrz@ zA%ZXmUJ8$Ku%mjTssU*iZ{4MANu|1l0=V|E$CwQLa0Ne5Z6171K58vg?!@BbBcOhU zHO+|XxbYb;b zs|-{AauFo!U(!pu*utDL&7qHXg(*;%C~T|`%fA<&3Tl9Lb*gkJ+F?~+H5>+B>*BrE zF1C!%o^Kqi^r#&P$KFvNA1U7fgLF$3zpQyMSelqN9BXBqZIvx&A$2-s;?vmna#s)IB7?k>wPAu&P-#F)E; zf{wBmDDYplh#DP4>WW*J{UIHrcC6^20#P=N3KL8CE^?8Nwyg9DKxV4lr)4+W2q29I z7wOP=8|!(}vYItFSd!S8egUGQ{EAP37x~S83 zzS;^|#=etw4dNwt>@$%DN|pA+CNrI|AC|EjwiWiuTIMCzXYd9(<3uzZ1@|!Vr_7YG zXvwfXeIZ7Yz}uX!Bea4xqq{z%CKkYd;e7#ggZ}6rFd+EFqMpvLjRZ$8 z!r%aaz3mI1GxDSZ=e32iV$P`+6EogeJ7|A0_tH#gG;q^o_wY(=R zGtGcUq4>S*C^5_4IzO9*Ele^0t z#iaXCYLs6G{x>j0eQ*UG<{Jo9w*k-FJ)ANgtTDjHx7laNC!qvs{=h%9Y#rYId`gKo zBp_S%(}E&A2|XsQL8K{z&5?-f`oD9Y^7_%jy3e?hhv+XHI8Z3pAG0SBN)Ms?%K;Y%twPz?_ zMe@xDls=J-eLYKi5p70bo|j4>9jvYA&J6DriD+VQRl+|VrrP~QYuZDe@#**;(yXL2 zD0ECmFkC#NazXp^DO-~k9cY6vXr4%7&h0z&&~&3YU@Q2%P5Q)Lk&;$MEY^kj1)2a( zX`$`mS^t&ZsSbqBAShArw9@p=#DOejgntfh`GP^jV#L~del-jzL zF==&L1}=R=qdsYkD01K{kzrgh(M!f}F_XercYw$-CA;P!wrxa)KnO!%_jD}rekuE> z5u%QQlGx*Ix2tiVGD(Nn7o_q|m8w~w9LA;-YAAMdImIhj}Nfx$||9& zuSr=!NAj1ldbgeb9>hn!eaP4`>cU>1XO%px36$0NOo`E#gPg}DJ+~;^sa|1t;$JAW zJr_-%F?$ayaUc_i5O}XCX%SI54W0^PAB}weg?_E{H&a0dwqlA>>avf=JSScCrA~>3 zU(vYMNvd|{?v3T{vd93$Dpz$N+XO=tgYN08^mxPE(<>DlEejzp3R~S`HH9pB) zs&GGf17IS-A|9& zHLo~eKP1w9fo<%N9L}+S80fc?5pgbKcAQSon$T;774Qj0tvW=R9+Lh7?*>JgvTzsa zNqtFWrgQ%iB`Z`*S58?nr{;mYX_`C){50v7mqCN6$zTk>gH|)T&U*Pa_%17J{>L87 zgyInsaQ|_x#M`2vJLl-l1(0vg2aaRXqoD@qM0k!< zVE$?)nwkfd4qn=lF|&XYZyvAjH$!A~_UFLBcy7OAY@chw)NmoNCZ3L~n9$2s;s9g{ zJ>x-J&k;#z*pKpqpj|+%6ACwBrKg8S^VBQ=ObW|IEvN-Dbq1pbEkvgL##j|5WA{5g8&)=m^@j zlx86kgCntdxX)y&XI~vZaOsAOnm!!$dZ%(V(=T!=c@weavb|W^_7+#6pvOb`_Y=+= zJ|xN90&yVl5oFaqMi>STHoOm*7>074ZgW|Sr=0B*XG_{351{NJDjF9eIB(qwEY0sV zMGjV%KDO&GkLG6TegWYP+oBUUzMOkl#n-DINZzQ+v#m%_kZAnZ?l4c0hsIl+H8t*D zn{M{`^w?r{Qsk`?Id5!hmYw=*4LF(^Q^o8_n?kk?DW#<3OZ8p~#f%>?W4!Ca82XGv zRbF%_mDn;9^KKP8aYCk!Cklf*nIaz=(j(+lMDUP-dr;j29 zLZ^UY-a2pULI*h0LNUWD| zppr}$4aPfn%NZ;JEXAPGJUB?qN#chH(77-}N-#u)`70%2)4CF*ELMA%a41#$v`na6 z6vEBxql&84?BSv>l0Q7qO=tl3ud(jH7sR#qmbv15W@$I@3hj|&J48z|8jEwltWMi_P7!t z-6*H(mwIyHWLopv$d8d7$P|Uw(=YGdkr%F2=pd;>y)l%9Nw7YQP_E`H`N@5vh0^NK zYh(N{L?8D&L+U)dZ-G8V%w{o-`qt+_+uIEM6{q*c@vBpi^vG~PW4(25?M%MNfO&4^ z_pczf%X{a=#dmq%TnnrZQ0Zr!_rP1+!f!0}-2A9U2Tg zfPrw$_U*O0ca)r2`rv03cO?NxxrJj&0kWMfOObX%Xy^PiJ~F)e^;5)LrewWGfJtrS zRl+fALp_%=!Cd27$b&oj6JF?i6LUz=zD~FImjfEZqKb%z7&}=uyb-fL`F( z%Ttqww}HGytJ?e3*B2v%9Xz{bvQ@69xN%I#;B36p!(3gl6;Y;#1-3Y7X!k>~2;j0D z<@yQA)C4gt?uIZ_|EUaIcg{ZaCPTcnQ~8vC1@b0HDli}n=@Cc*^yciA$P{EqRrWl& zy9mZ7NqMynG{S8~59k3kRq2+@7LhA$^{fRteVPhGo~5MN8c5bRcI__Ik&Jjfxv~ppp+bYJ13A*>jUPcQ(z&!j@OXv+!S1)wCMq=?oGY zTaQ_7ctvq4^nz3(7!3W1D`O~7T4$Bl0B;5JM^vo3GJUAQZfxDsUl+o51gVw5H{tNB zM2YB+0ht!C9zInj6!gh9p=VJymR8N$QH4=(03M4w zo#5ukBhVCE$13SGm^wMDomrPwDD|p?DM2V9n7T@mh%~=+KYWLpNV@+@K{?(?fI0t_ zdE&`$SrrY@EdkI=s*qig(_eLGxlfMdJ$b4iM~Cg(6Jh17 zO%V!AL!U-#gSdA1tEq1)-rnx>PEC?*NzLcO^~GKJ{l`>bsg^dK1s~sE=5@lw>`koX zILgOyA0bgFRirzHPmQQ^IH=%Ef#%~>-iptGx7mEmv{ZKX*P8bx5Mx7S-#fh+ne#H! z?!Si|D7y|wM}-07Voe3+r9mG;7Av(iy=J>Asm!;Z-u!HOYDP`d#X-9PxI#tQjO;ey z%1RR5IbsyB_Tfyc7basDJT0i>TfKN7+%yAg0SbEbp#}|H+Y7|=vc^pgR)moNrdk(0 zYd3oDX zA7UZ9Zr!Xc{oK03FdG|knQ#+LopO`dxINxvxj-36=oPjOmPgzq_CSxlIyf<@ecPWp zX2?B9dU*jT`d;uh$G#M`4r-gtp^!=n9aBA4x9#C~faBZA8&GGsk2Rjzvt^U3dJ$*r2bHo3t>pvey0 zL6b`3t-60pAaz<|XI&q4(_X+RZgldds*+w|K>=G%ZnjL`hkw6 zah+hl1k{DWa<4j~87^)bYz|r@!*8lnod

?p$B!$-d@==#S_J{XeuWJF%k!y_gEokft219j7+@RlLv^$%*|S7h)E$w6)Sn=6d5mPacpOM zVq8dSgU}${r!%J6)zWilE7)F+ul|}ssLTW{^?9LkxSF!Dyh8E6IZD1Tq~;@Ml66$~ z+v|b;Fz9NSAD?XaQsV0-BnL+vsYr8g7%!CBU8Vlzu!{n2chaS#>)yzxl9W#hAHs3} zUGVhjVIcJw3k|RWW^W2WC_J)Wu5y$36VwH z5y9HM;;k5m{&{*NtG;>!w7^Wfz}-oa0W5n8z*;~`Ppl|BVL;I-INy-`ggqD4OBx0d zc?L@rzK506wU4}FB~)u` z4}W@&LBJnni^_4wXhMBrE8)b0NEv&4%DCIJP=#>3pF@oO4W_fI-g_o|6opl*4g`m9 zbvtEJyl?O4<%?8drYrngkR6knu7Z;`#N$e&y8)VVY$?X%+7h`G4>7oVI!Q%YgjDfq z+X_cZ*R=}}aXR1>o71-=+v^ZTjje=i(6|(6lwySfJb`#ap-$V!tOt^CBIY||F|)B0 zQ+WdBu}LpSLc1?=yJv6TsCwfP(7RkNQ(LIigLYGJOn4iE9~dXK1~xNJy6YyvY4%zA zmRI^LG7E4UnwKu^*hJv?x;RXh9P!8FzH3)T}sZs{oi z`b6G5ssj8UF5*sJ9o3~-q`Scg@^1>^%B_1#OJ6S=!en~sBN#E3LgwH5##YBSKr^h6 zj@eca6w|f}Fz!VXZ*&ALz(Eo;RZ|{MUW3|YAwzf+snUdc`YHnA{zlK zv8qGD`xr7ZZ3*Zura`aa)3C;n#`1!DzYing0@|Tilp&Q7f={p5AoB?}-=jpveU;j3 zKT#yK;OG*fkshv(D}}?J)z9Jqxq}$WyQ@+r6BBTCG=mVD|6LxGoJy?N)+3d^(vrDh z59Yc5brPHD48K+XiCydoF$5gYOP7Ux4EV_tLH!F#4l^W^ML4*AgF?D1rd;_<&+Ibk zS^W58H}-~F?jvfpZw~B)8E%fi88hw>AzsRiYh$@VihG_IQK zJ?q#>O)vjH&yJ=YUhc;BkYqt58_lwER|LJ*m2-{&j;o>71njMbe?;MM1av}gzoxhv zLI$xOo+0~lGjr8Y6YspGpQc5-+x{RO7L|yI%lE|#z%+&a9CY>>lrTw>Q*BDZpVo_7 z0E4CXbPMyo`3weke8Sc4ckK(Zfc%8(JHw~+yY-kLN;p91Bh5Jy-79@qQCOQ`UiqR! z3d3VC1Bf<&z5!KYN#n>akTQJF>^ia0_)2aNq-W&NgO;PP23S0L^h1G+RZ6^J7=`4+ zz}>)rY^|gw*VF+Jn6prbq9QfB*|!mCw$t6FWm|&u67<8%!~u8;i6%iFu*+})D@GWwgRNbIhTmTLGssU&ss$-@(bGVxQA3ZHV3TYr^i3<1v?k{#vd8Oizv7c1o4bAB2Q#9#nmgN;50Z+JYFZ!?4v|0)#I-FCBSfeu#I9@PoT z!&?|?qBr9|vK?*!&gxSa&3I-}1E=ce_=Jc|{1dn$G`X+xI<(%Txe52+(H{A!-s2bd z0il@54|TyANM{f#a8P=9(vovf9qdz%n?GbcHR9MS`v~aj!UVYB9wKk}B{`)84zWwzW2-~3V!MU(IK+cqcmtd)=(i;R>Ig&Ar7^w*u2KOv84KVH2&j0SgQ zsTtj@qV}gzHv7?4**Gq18kI|bFxl}gP@Y(^EHeXaJUk|7a#HK%ogWary|MLeA|NVy zjHn+|>4S7~Ar_N~eekNA;l$4^LI*;uYNFw)#5cY@-yA-kkdYjz1R#A69B?1kP|7-- zc-t75v%LZCtF(FP=K@C8xFsV7V^ol znLx}fZN31}p3U9hBdFeb+u#~44LgG|k|SP^OO0DtQTzGNtnO2j$8mwj!qPI_sSATX zLnI=)L`Zbkm0thvKOH=xv)=ITKj?%*75h(uegw-fy|3zpk{Cy?+(vd%_Yv}8Kf@Gd zim_%`9Sn_ZTOOdL=+q2lB=Ui6m^y6ZKQ7*ERH$UwG)%f;x7q4I8GgK0sT&? zv$q{GlD;U!J))Rh(DjWt0da|o2r1N>EUES@NOdLI;`a%YowAqCB4>=A#)t{DWi~Yswd-^; zWP(%H!JP4-Guy6+u80{gYQP;>x{A#C!7s^@;SWPg!nu@N9O}+6H((I8zPbFA4Q~If zaCzG#N>AX%KyB?+vEL5XL9Bhx&F?0*^RZ*A9-zXgk=O?<#8r8Q3>4mmI~*O6jQMSV z2hsIzh{SOcrdNM8vZ7LdP1WD{dv#?9pd>Vf(B4*{ZJXgRAq{CQ^MSKw>O6)eNQd_= z^?TzU-xqEbNTm?STH8~_TM&No;eZ>(ho20|{CfnhKkkSNFFRo~jbp<_wPhf7|qX9?} zj5mH?h7sbeU^;Pt=p!w!DWu+V`5Mg}(BlW-l#$IQ+ zSC}iTXV|!!;lU%)FGV@&h(rtBKFyuXik1<_mw356fZ3D!HMc+ZZapfQoJgl<-K1sR zuKNB&%6Qo&d|iEdChb5&dhQ$h26G0??!20W4Rh?ATO4Bwru#}Bxztk9ZiLbVAI?nt z3wMbtce7h5eq!N*N3DI}1tlz1+0>(%ovOmqni)z2Ovz%*^74ztc`-?IskQ~t|1W8! zXIk<0G0`1k8O)*Bbi<5!hwvAjXw6$PfMT`tn<`A1dj_2<^%$bU6?&|<8f)J|$x@E3 z3>=N?~~n7I#TXL^)TK(RTlJZ9k;zyRv}v9QU-aZ8Oz>cDXXBWAb> z8mWY=&X9n38Y11{vr&$M9FdSP>G;RBZX{9{722_Wc{K2yC&SNi;*kE&(b9tF^9eQN z>`~tYk)GbCr!zhEibSuG^U%mbXUN#z74EYO7vvVYIm2f1(vH9>_~ExU-Midt75tgm zbS?$>+&&WH=5fJnY=dZL_OJ85Fqyk{i9%t&xQ9LKZj6RxH-aEo;c7VpD<>vn`qw!P zJXDEVxOF01YIY@+wi=jTH4_LC7W_SPWe2eVsexN52${?890&))fAgcJum;5`F z_$_@uyNm?vu&)S+*XO~0Qj}~t~qM!31^ugx6EMDHh(q#yfGn_sp)Uq>cOlt~2eS+&W6) z7r76Xo!a?S%;gzQ?tNKUQg}Eb^4o!uz1WA{+g-eTg{VRSycZ`4;KwXi- z@A>mV8?5H$+qA!LO4(^hq4%r9lHsY{T}b!s#VO6H2`h= zjLW7Bs8$oGv}F)*=qB^2`frOGMm43H@EDG>Yc+$gp$0HfG6vn8j7TeVBuVsGr%uy% zW2OgG-WB*Qe(KT~NyTH#a2x=;bg^a|-aO`F!nm19_-}UpOa>p&)gp>z5 z_<##gL>_92BqHA*F$3a*(>}nK7*WemjUWLO8Daaqwwu-DLr5|ZS-fEC2K;cb;B1OA zDhY0F`rCl77(MP?E2fsK@JG8y2UonxbT&~}JHR@OqII{jy(r_R3hE>IO8XBaJHF4v zWGks}&ZgNFETJky2UyPfPs66jy3EPjnv~CBcAy|`bH4%_GPOw>!*InDcD;NjN@Le2 zrf~JNwyifsh1lrRm3BoDlH;)MqbXt$}q6ww& zD#D}bmTZr4SC_vcxw7^$%s|m6J;E?>8RWehgest~J;wMM$a~$fgm)^PFe>j|^#yeu z3r9ur_N&X%F7flPuicyT+}%c$WXh`f7W6RaDc1EfmFo;P`j&10a`MDi&3xhfN2Jhe zA+|8`FoI<3bn94BALeQ$e*B26+Ue&r)j9U+OBwY%flg6DoLHt!dy^&ktS~JDx%PCn z!?;d9TBJ-26*(<6FV43JIRji|eA$pI62|%@CC;aQ67+p8OpaP|C?_>5{2#as=J*k| z%tvDT4QB=wUA^8P%_{=HbbRIeQ;GB5u_F=g7ss%0D3%s_hTttb3(Z30N2O$6H_I+Y{1Hf_PGHST zbP2L=(-wAewv==4vQwrr$X35UH z5^xWumqvJeH;+NSr}HA)KT0-8IK!)RX27X3u8l_A67lW{y^&-XlgzoI%MpJT#- zbE}&zSMJoEr`$bBt)mFb&^#gfNI<4bLet-4C$g=*axZlmUa}l;_PZ zpW$N%Op&V}8 ze3XjtpA|?0N4+7{G4n6R6~{{{sOQOj_X{vQZjU{H?uz+$Ebff;x3Tmbe%}Zr9LZ)Q zYQ%UXL&`AZo0`EccvYAX3G-iOXT&y`AH)m6#d>2Q*=ILZ9*I!lC61 zL0Q2TIOOd98#;V|uZnH^ust!_=eKww=aY0x{9Q?hVP&j3{F6Md1P2kHC1v%(4x-ws z5!vT4-D#P~fa8%UB5FU@&27;=e8IZ{4M`;}lR4}onxqhuIpcodRrrpQXLA2x!zn%d z0ka<&V1^J=9ZLs$$jB(@8=e~BEapk588fjxR~;Nv%$(UaK z<=KGsfL#OGP2`hwQ}PC#@(h5cOe{=sPICN?PN0|J_Tp}4QDGvG@9c4IBPUW^73K5{ zQ`SkKvs&jMd`Z}x+y4i-NROPXd~c3c@$#+5*3Kcvprs{g!I@_H9wnGrw$AWFATrcS z+g@Lr-a5G=x!gFox0YlngQhgm@7#!VJ-{x zys3&9N9=+jQ!0G{%C`gEHslngcBK;T%ZDx;pljV~Q6a3HaWOug-Sz>7n9SisCh{V~y;v3Vd20^|)Fz-o7JPA#!D`uB~ zF(ffwH=e)kDZ8|M)bepjco69ZIBIh{v5!f7`#n+aHx3n9}*t7sRY}WXj*;nmJZtUu-yp z#zC-rb%4m)U-EoEzn$=lyGnjXFmOHl{p*`d-o;|G8+|+b}#ExS`l8O)dfWkWYyqXZ=Jns@!~^ zt8yNos@3wGXs@qr6rjwl%0Nx*^z{YK?M#$7^o+{2e@&v~l4}B9bbux-M^T(>MOF;SaF39DrM6cRdMSA4C%~EpWJdiT)dYs!qAYZ;9h~OkXe!3#KC_I177~cmn z(N$a%nNm^!M`jwwA-Jl`-@hB(6I+l(M8T(=6<<6s$H7ifXjndV&jQvMo;|eMgq|+? ze3V!I$!ZUSE0ovGZV--=YM02&`4r5;EI^EGjw>{A@{20oFAbO}l|w02RY0MI3Hr~% zi)_;m^w_sI~>xVhmhR`-dQe>Z(eUE zq5dX+E-1jtMA;e#s36S6A{h~#D99Niu5pqo`B~dFAcRAu9hn)%H4;|;m*(A)2R8gS z5Tq4kZwHDtSk99AuJ;Dq7b$Vc_a{p*E#TA^Rgew4aoN(R_Exy&Md9OfuG!p+Rfi-X zFvPv%Z`^O_c?nRt`F$Dd_dzK=QXkFY#zzl@xInL_kVu2E(~VFwkR>hS`P(+5)&hGp5jI z5)Csq6BqT#T1MZof+EaNDZj)2ysoyh;-T%TzVkO?49$stY2?BlOGHbc8gkhkBb`_~ zxM*M|g{MARLQA34pGIjg%&zM>wnZD&BEqJ3Z`f9x({d?cF`_hHSklK!75FStb8(9` zg$(l?Qivsl{b%}VxyvILEWj7FU77k4zpTXFwVw? z_s)|PEaOS3i1`tjNXtbEfSCGs?Sn>zAaTH+addT zt1kI%JjscMT3M=2`r2P@9`xUav>GekZ1>ZrN9Wfw5S|Y|WoF&cmN5>#Bf$m-4*5FN z#4ztoS&Ax8!;ShGIRPpuB3r4=kJ1t|bBjF;mmb8Da4xe$oxAnmA`6VP|Fg4+2#%es|IVbSTLZ7N7uHoa(N)#Z z^P@D0a|UXiG)FCn@3perU|&zABQ+wqFO=y0DhxTRm1u?^ z(@t@A(Cv#mgD+v6jvqaTDg=5l{P?_0YBb}PFJDUyuu9RhpkZ z?_3ze+9phPlA8k=v+GIXnx7{>%khVp+VTsN6HN(Ca%qVeNM_~_7%R}KX;qYd2jPF* z-PrO2*cjbXpg)Mc?4FyfyLF!1vdpKRk~N6^N>p~Q^e;RAc-hU?3pai58|LP+h5?=} zx_T(l1AwI`QYlgS_hwtEK1QQ6$MPLJFBW#8%QZC12LbS5lYLtJol z)%n2Lwc|r)7jYxFb4{NI-J-Qa$fr7y36Xlj!kiw}5aQQUu`2s^^B#3Mcq8B6mt?!1curcH3V5Tqiru?`JXK|km9kJDTv#23U zn3rlWI1}9T8eAfVV0nG~-EOfXqMfrVgP9 z`HO?S=rhLDq$&H@T^aj%6epG^J{Ja|0^0nexeZB@nu7~T<$RC`Sz&}6m6On1j3!(M zeHJXHQ!CX#Ph1=@5?faD5Lj9`6SoGIb{UxaJ&T*rWPWMz%|N|8kpI!ehKnylM>@Px zzmZD83M02(p|Q!R% z)DDj(V%b*SnjKM#90TPZr+=h#_fOj1KtZ4DGrT`SrGltHO%33(Q$qiJmfd*vZ+(rc5(Y!TcUSOFa%Sc+tf8ompj{FK^j-*?$ zx>73#R042EWPMKJO|COpD>3wB2uO~h8e^ON(Y;Jr z?_iom(m|IYA_$|@$XYj4m4pqW3cc;>{w+a_mrlRhZ0H`T8t~6Hj_KwTeDAzFLJwCkkm%8_v2uMaAA3Z_>{dZ%k_+K|Sw}LWLIG zh@bJb51KlOBDGOA`#ma1T{d}9i24S>wX9s>Ofd?x%L_(S}Ht31i zBTZ2QdEju2-l`pA1Q}HTY-xB=r{y8-6XLs=8~ihOX3WPf!O8;cDe&5RWXtk;pk3og ztT?S@vh2OFq&n2lSkC8X+@6?md;%Q7<>W8pp0>x4%6nZbHmwSCwzke-S8$0P^26%x zK20Uy8pBDznXlDk9ewsrlq!jLJKaP3>Sgm7_z~>%=YCRR;UgAkuSc4%%q**^n_{}B zP}s@?YeO3g8i>g&XqM&^lda1aJc(+Ugmj^f_QF+t2V$WL`~EDULbei5^QalBwVxJS zY0n1oEdAr&AFT(&KQ2KpLF^*DJLt< z^x>(?`zb!$ZX%eLNg82}t)wQ09beu82Setr0i4NC%TZc4Q#gAqiddyPrES%{sYV$? z%yYz&iu|Tgz~8o#UcM_4-5Av(yX!q1&OO#Tmn6;fsu`;rW^s6%y_+%nm+R!8eBg9% zhnN^-hZgBqK2eT(r`paVImee68~C_k_F+kaGzWM1yvI$LBBC6cG7GpQygFdWN%@gP z8Vx;_d7ZkZvlo}qJa#>7{eq-vxV{A4Y(nw_;$)vNFP-7b^|>YtuM*rB`@T;yNQ;o% z2+@)?m{pe#6cGr=S)>(zpt^BL9aA*9m3T7}3age9a_gK*j8kZHXf_CD<_L|V#D4}@ z^;EVcz>j*+#nE#5cI|P5XOhrj`{UQX&8Wua0XPk!{u}e>-}~gewHb%vCMNd_#FmY=6f{Cf6KwIZp>(7sj4->kgg!S zbl)iK3dkd?&cQ`646i`narA?*S*KVDsgifdNcHw82zb$lCf`YkVA&F6qo`gtmfAbt2MYsLA7hDwj< zHLO76c#B}KPY%-lIbQ%nK)kX7Wfp!bmQk`0Tni}V>`ZxaT zVuBPm4M?SS=if(;+#pii^Y($ETl;1|a~bOt1@YA-aY z8Asg}2qR$6jO1A9om68zORqS%dk3cIl$r^&;JB-*LNEc z8pP#{?ZQl*qi7o5g7EzyYS1^{%mW^Tl~1`rxUfua>USn!L?>}FD4V&M`E7S_O=3+w z_hYC3D7R)cO0M00f6VL6nJqI?wM2B=IE1|*Ba#Cx zS>3#=)QH{`lzXoo5SVjEWZu8K>xZ+m z1*`)(x-ZCShc@HGN6r$4;llj3b)jdq*`lkFa`aX zdmV7RJq!aEmG`Q_()+eq`3sY)eTRFFDqIR`YPFiR{%>Z{N8l>TU;F7c`0x%Pd4mGSuccLgJC44 zGQeI%ArIL!TJEa8p_C{Rdc;N(s30)EPzv!dl2DbkSPhWozc5UERO&wTn#k0jp z>)SO^f2t}b$MsPrK^spGYi3zGTIv#^zzGBQ2Jr-MT>#j+X2u+&U5!?--CLU(t_9v4 z>(3}f>~hP(2t^KM#Hr-uaTk0GHO59)XQQV*NDmV%0>H~>oMM5Ma4RJdvY?IzY&?H~ zTe!995nR+$E^e{ClP#YDAO;i!G=Hg3{h0A+^=Yx>9lo+&ROjAr zeUUY+SBdu!#4s8_qiKQLFBRucV;!?d9^r}ea+u=C%zs`{2& zo)hFukCV(a!-vYz-Cc$4+2sZ5Sq=}8NSHbiY5VlQDPkZO$u=Vu?TkSB_ zPXY^8N4h1}+F(+cikgTKHC5UOMBUum18{C|zhKOqsKHMS+jH3Cwl#+Ewk{eJQ|O*$ zQ_T9%K|m{a|E@k~UMoiUmus|dhHki|3tA=oy-`tl5Ji=l$^S$eVF8ELh>5NXf}T40 zWP2v4=P<2tE3V+7`;9@AWx+(0tt);PLC)gdU=BNF!^J1qrkU5L1dcMS2Sgz<= zcCs7=pJ0fW3RhDA+YI=77!#B`R?n-gdh-pw_OGxE2GlG3;s1Tz-WH8m4LJC$Ijuz9 z6Fy_qfK(cO*qkewpbFjLl18rxt>qGv5Y3rACfo)>`s%*KZ-zyAE<`{Ud4=Ie7*Z@& zOg7-*W15I| zUb<7mo%;&jpxE#rG0%q`EpDyJFsg7zy0sY&aK!{e{xJRQ>pU`$aj_L2dH(<_mCKbe zz`Fe*N%~Bqy*Vd37p-){{<4C zokso9xF_qwijFX!@XJpvcSrcvudtP#?bj9U(X4R6m6V)h;-?8vMVr}3yh8{yhw$3B zJ>P2T2052CuEK&Ysbj^j+_VB+3*I;tvMfH@F!5@dn5`pNzF1nvK4<*u+RU%`(@&-U zoXxRWp@(^{*vjbJ#a6Is)Yr<2beuDwc7m~{TA&Jl`W~B>B=-y$lR?NxZ!xfj`0YuJ z3nCZ9U-xU{qLOMeIs)s6=C!YhEyl{B79sN6gSTomRpKi^gii&wr8m0!I{LNvV+3$t`zoeOrwf#QMwMVUk)-J^wci244 zTeFwzdgVd&TniGa_*79`Ekn3}ixrH#Xz=5i-N|^0$DpMR?6Uq^n!i0yO;1QE`qEs6 zuNd3oaao$1Kdky+YO$Qk!Px!$V7d>R*#RP{-k5us%=1115%LJlCKv?rkGEmog=a8# zYbUl4=hWaI9?YZ)Hb6}8%mG-xTb=~bjTTEv&tP=$sT8)o`jheeKh?vLwBrOmv+R^V z?MVcBMvmBCnNH>~b~N()e?6&7Wmb5N7DieG5Rf7Y=yrAK5t9pbjR%%=t|wy+%H z>E{iMV&kV&C=R+)Bp#Xtu-m+=hawsvfzh-;ww9TrU2IIgTXJ!*PK}C&$#RYLFXJcKAtoON#E6R3h*&L+pcfa^WG;m13a2sYR3zG6tTHa3N)d-WtCxj3Qr%=@M$qVOzG-<;OwbebGRqqn| zBzF{N-%ct8TqVq+o?GuAQvZZ7!^AX{UxD~H%AfswS^@|~ion`Fa3NbccV^cqVM$&E zE-fvqbM$_lvj30{cHKPFR%794A`sEny?FB00gJ)~z8C$;Mt~qTwxeVXTS@nur*58G{~_Lly4m$W{w7&c8A^6j zlDtyS-2HEyNNSc)=ebcmiNy(T9gmXH9bXimy?8E@avJAH_PyMDPxEpH+<5mLzfp7l zroQJpTQO)sax-!?T5J56E&T1^5`9Fp?xvupUE%PNHP9bFHhG`@z+z9=+zXsh301BJ zn75oXU5w9v1qo5lcA_?jc0@rl{_f=EJ>jc%T3Y)U?9yY)(`?}uhF z@_cjBvZOn2;t%7m(8j=F+FVymgUtR0$PS6f*QW_O->rec7*hmzb0@6WU)85j`0e06;IZe*9fvcS|WT%M*KR#cuitlJdMq{Sn_IALOsq&^hN5F z>yrJi_G$pU-E+q)HwboCj9beb+Eyx@K1j>)l{V+rUF*EsfmicP^{mTqhxiWa)T4e2 zo?PnPtE~ow3yvrDnY9L%u^jKabz5@>LU(2UziB{vQ0KF^&AKH&wQX;LFk-bnj4Kh~ z+XVSE-FZU^@I=%pPfhr8I&;%FmZ`*YkS})nh^dhX018ujuZvbnM)5<}szJ-l2^JPj zBvkk)L5=G}^?No18feI0x574Si~qvGh3VtOh9Ht57YFF|(rwZrPbXz5{Wg}nX;H)G3mKyT2TOsEAODQmConw{h|w#tTQpMF5Be-v9ml8wDMlvs48*I29{rWB<7Xetq8`IM#T=NlERN!C?8vsvuB!s zCM?4r$(RPqXmI$r2X8L!=zlj}%<>|=O-iHiIO$Lid1G|pQ~I|)kajpT6kJcagaD0BM0q`0x~;KJOx&Iq_C4ADI5i5kEl zhOWuz2Iz>XdwhiP$q0AVje1dCVm#T16ZnOKQn7oUF4`+Q7~27PW9hcaj5iQnj@vz) zz4;v*qoS&gUyl3+5O+qrShi10t|GEUR3M6qYd9$}-r7s*x$_Xll8Ael3me4xna%?@ zWV^yT_2>r1<1uH14?LOA4LAs}o)3IWmTM9@N~$OT7EU}uO|9r1YrfIyjN!upzWD%+kTo<+CsUI$O8GgOtR?^j(Y0}Mc$8Q-vj|-r z$%xDJM+aXCB~GVcJRq>w!FxqinHfYrV9v>#BiHeE%3l*Np~VRUUo668#Sk)sY{wt?t;ms5ZiG(c4dERcuGG{#P>Wqf1&>tj~V!q#|J4EgaRdN~gO`VXkVK zfqsI#jzP8`CzEu!9{1p$E@ft?lK>G~_`?Ebg_(0O9G&EYqIx1dB9YfoOiby8_}Rm8 zOd!`Au@=F_UC$P z0IUA8y51O%MXIi0`aIHPU~=T@($|bf^do-MVST6Oz28skxl0Bpi!CJaXo!mG{mw(L zf6ojEZ297++Sbw(11_Sj(+^-%y{Kt7pH7RE_3544cH_P*a-uh)LqMUF-XIW;;&Nlj^K9eK8qJc5XpZ(24#INCs19V0%wkGKN3Ad7wr%B6f<#FKvY=x z^dDHUk!?C#`-B)HL@lcy8qrNI`89RLP&1*_p11r zZAmg%IzqlYxGW%aa^7t4FC35tulK+hlgB%S^Ub7PJz)k6G;os*euWV-dAyd$_|#O# zx?9L@;6qe|u7I0qNnBxBMZtc8dl~kW69R(1ir@1IYv!D~uC{Kr53N4H%FfFhrvn3B zBmdl6x~Ubt)n{+~;A2yR{#t4&Q^f*M?gUAS`Gl-jDrJI?gXsije_yTgdq{zD54FQR z9c~uV8`qiEH=%Uq`rGxxR-0Il2-83P4GYYeL4D^UEtbyszwa%&=r>HxkPBZGKt?nX zQ;?i#Y{bg8bUqX@#f(+AJhh~s67aI?JLN_F-mi&{b$sFYE`i2ge8ERM8EJnunW{xB z?1hM1Sn%CwkfZQxwVn6mw_-C1eTWg|(~w@Xy$T<3M^FnH5`jMqyG@a1!Pi8hc~=(( zUZrWeOLn5iszYV99o6oD{n*q?cfncfq{=NQ+LyS&p^n1^EhE&5J-` zI1l^9uDyz^E;J@9(ug7P-Nit}+O4FP^9Pg8_nyc5^YQ?%v`3pc^R;M=v zq0IsIzl?F^m)Z@mw20NcxVowv*SyW6?&?bWNWIl~QJo#5I~qvLOc!0$-t{ximN@7L zM1>zzF+*MxC@2@-IAuq5y{jI%3ek}vx`)WY@{5JKI^)cs-rKhXE$n+tccF4r>Xg}s zJ+OOBnQK?kf5zHQ0$XG!>WRrr@PmocS8y4>#ORb!6j1Mn37vmkH8M6xKVM*zV;Znq zNT!*$x-ARl;oYuVjKuQ_LMQvF)r>R=WRUvM^6_atMty;rh`NdTe;-`FNB?yHFKMey ze9l67g+8a?6aLfO+lQ!S?I*KMfaz&2`rag&dj5S3{QqUrpWFMz$trSDMyYvY+n z;QI4wf||y&BdeAW8pjBUerk0PS+81FJ(3vLf7bN#Je)uw3$iSAu+;y~5Utg$;=;^n zji*?7FEAaWxB3yNnc4r-*uSr09CS;4wga_#m<9J)HcJ#3I|DiZYOfIQv5=0vTkzbL z{bPAi)T734bJ-3b8Imw3ze9*JXa+*-+~CW0Z+U0pLeGJkHBAG0xi{wRdQaOVCwqL~ zDl-{<+^{{r*L2St7k#I_jIoZLHYvyZi}6h zb*K*#CCh=I|8gvaxG7l?^7W z%sVu9QxcR{gg!}{6+ptKJk|o}?MlP|LfXZumIiMAGW=iK*&mU6jH0HUe^-&1ogT_F zGJkpzi~?=N^aglMc>_^+Rbn05o-*$D$uV;=aNR#ZU!aq*5UiLMQyBfhNC&`o8abNy zTsbQ6=Tt}q;Y}}N_#K31IhJ1oq#pj~Sg*Rk_<7rrtX*~8!|uo(F|`6L(6%LW{-l{T zOa7>qG-dy|O>DYx($_PDIB@G0?gmv$b&QBD)xw9k>E@5@%Y!pc2chpz*}66))2u`F z97n-m^Icj-Pz`s;HYvzeb|`Ck9#KHlAsjWRWVM$l-UrG%={wF+-4S*3k;|fYCxATX z6TT#j@TRQz1oN2N!+tSp=lP9I3yfB;U~L1D2^$3=OOWnqOMLupXF^>^3@(Y zB>+R4iQToQbbwoqqdff>rxdIA_MY@bR4JfBm9O9?z}1Ej)N-8Z)kye&nwb$;P}Po5 zbj!GJ!&o=EMAnckM1w49e*F$F_|ab*q}C>*0@Hjbcg}U?(UT?G-|gW*7U7JJWbYQ$3(4+ zeqgDekNwv8GtTv;?$~5Q;W=x9kW>CR$^IJa^(;;eQ5TtWe`e#0-3=BsWqNNdxVB&^96`OmtTA z3JSIr_0yXOla@uJg~-6)UKF0b|H<7OS$F6g28^$I@QdUTe>^HiP}$~#>fk6lMgh`w zjzn^QpzGu%bQYgP0NBc$ZpLw96v1hT7|&dqI8>C4_J=jE;yRVr4*A<5kJ84y9!k*W zxQh+2(SwGK88wr$-hzoabVhts-61h2upfcJv{g!mn#5xChUfYs!-se&xKDIQ^1S%qabFsmH;_P@_zSM4^_ z>qJE3@0P)^+T4O4!qoJ}SKF!Va0cBCk?Bx57x~t=ocBW}F4Gq4+cQ9Tgel zLAN_vXJ*CQiQl-&gDoz0(0O-r!2Z-(t)k`sBn3n7V}6DV7b@KWv4aJQ#fMj(rS7jp zpoiU;!LMFxa))pgpc^4r)mWjUvC9bknKdh2`B^B9pu15v0E-;(D3f^**4{cyB{Vpn z1QWCm*tV_UdrjdYSEwnVax3WrVh{opPH>25M1*22%pKv%&Z<1X>H7A=P_d7#MlwqV)?7 zNUiZdf`jJ>Lu{S*V>a>_?CVp59pMh{gTCFjt&`<N773y5AE>t-s|2{`vC5^3#AStxWbV9k}l zO=kdvI)J{TW^{Shg*P8~hV0O*dhc@01Luu&8#27E8l$Q6EeKC|F%^fJgd+V+E;jTJ zpE@J4)+2h_Y{%gKXk9EWeQTZhN|oz!ZTEwNqc~O-z*Wm7Fzy9~FaGQD0?LT|9pU^k zK3;QiWx`^=1G0*yu(h2*F65#2er{{_ zHx$J>HKsWm(zc$tF4J`|yGZ$=p25*-Fo~DyeEVIJsLv8T*NmkQ4p=aXFvse3)WW~- zQko}7%X$Mu5QeVVR_1M?@uC}QIc+zAA#D)#2v@ji8NulZq$_UMtT!-m5C{@kR9G@A zTsK%`^JU#u0fDJZmXNRdNpt&>{xl1JalWfnJfCjwtX-%SB*w8CccN(dhDb##(PxEj zYf*+2GD9w^0e;HDW%?S z-d_=As*{@lXQg^-+5VHBc*s&aXN^VIEZ_t8i})tdjLXUQQ?mpQ`tW49gi<%1WGvp{ z3(3J{3HE|UEGmPH^!1NuJ|3zD19m|Jn2}{ zaYxn4iO>ndp0CbrWl2AggIv?+ot__?!%Mae?1PqB(+7$e#1Lmc;xO3DAgBLPF`@O> zT?1sw_E=zzX|sN~U{(TTj-WRw5nf|&a!1gWvyBzL+P+@hSlglN46UG&LP@ZUVN+@W zD{L2dB(ikMXRh3Reo`>)^=`$%yTfPM&YLZ>*FJr5s#}SIVnj$t{vAU0hx@j0{@%>cdUV@vOMl1Jo;rdxi zI>vTNH$I#qSXLw}ugi>vCn6%JWOOY)cOi|f2`;H20gT2&jEs}?F4sny_WXi3rq{d~ z^d*OH#oh~Y?He=@;4uW*ZaUUFOygVqpLIHytEvL$4hxx^ONH1Gm}u&lZWv2|pB|YJ zW^T>owZIEKu$15W%KK1)3B0yy5qJC@F4z5g`+)e%oP!dubE{c!2Eu020y)9t&ySrMiAPZf{ zEx@2mu#M@thv?62N0Bi|g1g|tglX}$`?}OXcX+@+w>DXF@Wv6A2pmyFjQ3Qei)?DM zEDrP3|G%Be2U<8SmMYiR+edbbh@c=D8*JoYO?VWia%G5RP>uFTQl4*jQ1nOYOH1YQ z1ibrWl4~5F7h5(=UUdWIAG1eIo0R7gqTxLU8;qI42mz3^cYqe>wm_ z2M|67pk|H?Xcj4_-wo(~odFioyS?F!wF)pqsYbuZ!P*EnC`uzNeSD~h!_NPY2S*sbii8_iA=p*PCR@VeSjMD(E*x`xfh*=|~Ke15k z)c-hkKzcDJSC=}?P%s+ab)Mn?hw%q7xG6-<`+}k9oP;OssLN=#v>Z&9yR#Zd6JxzW z2j63XeV`c-bv^9&#g@{MuVKy44?U$k(M>65y!MM#JSQZDRGBh+uswn5fG-LqF~k^y zY>bC7tIoQ3B!9tu&CEx3^ z-G`?HF?mtV3jXM{A~9}YzCb4vkH%Cu(L!S4r9x;aIzIc0+u`GhlTMx!}4#Yz#Im+HL21ZJoP^kkBG>@ zp%lLuf%eslF?QfB!-ih-YvM^>f+e^6j0c|c9u*To#AFqD&oq(dugJ>`5PQT@-<-Rx zaanG3fGlh7_RiA?d zXF*G5(*pA6;Q56zzCsze`tJJ#uQ6<=u7nf|gxG^38OS$(cxc>A3o|u7gJv0#7T(Uu zprpGmYLi+?z4+j5ZPNrAnj4RK>|qSu$|(nPYK4-0nwGs>fs~PKP_PA0 z&KlC^Mo8|cq>jw9Uzrs4X=89!u0C@OplXrvbn3=LO(x~UVc~}|EKE}=S>d>@&bsyZ ztE?^mPtg~JhTWNucN5PFxM^o`AkD31m96`ycoGCF}(YKzfO z`q|3Sh8(0 z9bZV!yf$xu^>hf{{YrIxP%>h`^`8S!%S8auF*yUIy+pOrY$3iwP;03z8UmESI&)q8 zFx3OOW$@#c9Br+M5`i4WfN5`pOkQ}jgOCbtZ|s!!g^LPJqtHc5aIiivSz=9&36VLlTMy++MYm~vXH@DNd6J7G9OC?-_F=- zMdPEGJ;=OVrIg8FmDPD;3krUR+BUkKdsW8)9d~#OZSWbmA5N|^5k-#gix{z1@^dIf zFCXX60_;-rwq&czOGh=>ovZPVWA4Ft3y9(adJhu(%Q~qPm{;^F8BHMi{v|&j4?aw6 z3vb&s58T;YDQ=-36){3%4BQ{+N^jEL3U6wFWzblXTkZ3~=?#V&^=obdx-Ni1i4P`ntA&2+KR--y|Qmq?U-(Ooo; zeYLW)l~1fQEHz-nfxv}GU7D95SU6m``ddWd^lZMZvaG)Flj#0U^_5cBr=#Hm8eZX> z_?G*T=}$xviSR~c!cvB3{O=rBM%Q*UA!q^1EM$JPqTdhB9TO`vdi}gwX(xT+KTTS( z5F4NFYZ_`MQLu$sIDU>M?{{8mDN1kQkz;HHIUJ^Js{K)#h|CUw14bgv@{iYe%9{Z} z7m=&7?$_Ys_V`1L&NM7q;TT$X=4Mph<`MWmJyU59x3aAddzUy(w|~U66DP>YqqX|f z;~l)FxNIXk=aJkS4Q5}le$}1srj-6r6G=NfHN4=|^7y(0-dy)VM0twTs|)>x4jvCQ zyX6|G`r>2ikx|)^E40iWRCuGq=*U_teoig4R()e*-Wu&8bAdI@`6CBsNql1~JcS!c zX~Yj^&vwvzM7{E)Xy-fCnX06~DJ*-wczMe7q!YZ@`R7p3nY7?I*$b1vDjxET7^@5mu>wwTvBjwa55vDE*eoJTtAYD| zFz1_4Ud8JRi_{U^=3Zt-SeVwr!ex6xkix|~DOyc^s?%3b=++_rSPl{-HuF;P2tiR| z;y;OrSf zcR6MGU|FXo#=p=B@uE@^X}nJ>FB!SrHoDb4-nmcIr5$UpVv?v4Mwy=u0xJtSPmtob zIh>|k%5Xd=!YR2S}B!31&!%wKl5f~d2f+=&sxo|{JL_Qv{66e^B=Z;+A*T|=h!z|&g%zrNIl zG@2e47ot^ayHZ(IDbQRxf=t>iOe*d;LjRbFx%>W8MQzRfGat>A3Lj%)fM)I4S?9Ob z3rMRqqc7LdI#mj%&?>$+2%iYop>C6^KSak*_7%U1S-RsZOP`DD5m5GtrezWTauFbZ zN(SIqyA&q!0F=Zb@gHZ&#M8P<1vL4`caOv+lu?`bfe4*DgLzakWe!x}EmuA%k5Y`t zMU!3Z)ovDfCaYoO{`mQ#vmimp;38|rTs*GrBF?plUA6@NOP6uOO$wkXJK;a~AjxsP z!ry2>SJqi)wb-)wV<$tGzJgpGL=9x@8p&Uf4K@W^K)^|rWAc&kJh)<1C~;ty^h_MT zybCf{+g|-;tV8g(XTnu&>jn_TqiX!JC<7f}=?eX@{TBG)Kq+%!UWH%nK1St(lstBR zVdZY?XK*Wdx%pC7Xl=*|?0bBZwFDizbF~5*T^@YcgE?!1JFu5BaD93^q|{bRzB6j- zPE;i^JkJg#*yu`Ax{((=Va$#hlz{4yQ}%?|T{$+FS}%QW2-yiKlR2ZeOtSQ3KgY1? zEBozhI8FFwZ-VJV_tV(q%^=)Yfy6+rjceV^L=Y$^>MvFMMAh+&+JzVr7b7eO^!HLE zc;o%DiP(P2PINFOd){(x5>e37cUX-QE9lPLYmLKB#x8wQrCQKzvWzZ2qM={twUBqm z`$4+Zg!2kKS|koSVuBRFM+)fQse6h|2P;CLNjKYw331uIij*@!DDxMJf%O3MpT4<= zMG#X?qV-n5A>CJLR(e#&@+wa|TW2H40FP&2)d;Ev>jp#`a5)tUs5}%rGaEZheh6?c z@1aj!RQUQIcAx%+-)e5j*_IeBfAqwEDB%ytIfW!#N)cU>hZK;H`0l!)-!kuDc4z^* z$&?r`IW#Rf_9`!AI91JjOsG9`t6Q37{*JqeYh6)OP7z>WoD5r<{YO546fA{%`T*7E zzyQH)oxxyvF31|U;zHldfCmH6^b4kurxrQ8xiUprFcV$T#NRW@GTLIm@C&980BQ!v zx{V6%=c63#MV~jq4yNtBa|Gt-rJlH_)r`U zCHkjyyqphT+t9NSs{d>l6id?Dd83tV-EmmTC`1%>!dUdf|5yr!<+{yhd0-;#xZ>h7 z?_2&LCQjG|eBGa5*fMb}QRYX{1aUPNG}gtLJ&jXuof=b}A%Y1M!^G;*gNFt?IYG;B zKNcotNe-;WaAu;k$ckhjl%kYwk#l=0+ec+4iQzd4yre7ee#bx@5nXl9AS*;&E%WS3)qJ zSAc;ht6XlCH~Q-=evdCSFiR#ML+14pZ6IS^)Tt^Qw!mHvEcp8H`&BjmpWTsDWTA#3 z${XeHeUex`_D zl`BM$&^I=&YfeU40-_vmEnF)j|a0OJk{HKL> zbyPwQs))M)OQj*|LY~qvTeBR#X?e{ZXwU`YEdxFp=AP( zal!9Jk2do2(kV?~{x28iN6sv97Rh`Je@P+Tr4)YuHI~nsJNqM5G`VGk+4^vnTSG!<^Om|Bj{P5CGxOgO+4+QC}sGsdqV2i<83#RC4o4QBDdt}ZojUkj+#z!{%~j7p+xkT5{t8yVHKg!7GxUk0UV)> zlXkqTuHw-CH6z!E;hTYk`3d;-MvPJ;jJd3i3!KC~8#H>SAw!1K3eQfm0X6D~x-XgA z;y%TPvs}S=ph>mPw8LC;&Tk3eWIkKS6(YLPTXK5eS9nUG++bbyO@9*PotXl_DKIrp z39Ju${*8Ca7@@mIxO}VPM+=|7U@4;fM`nq#5A3rX_8Ba?Jim!?VLnJ$WMA%Xc-7;~w-Gc0>X5JY_M(<0taFaIRg&u?-8=h%Gs3)p7 z=2uzKnH&jXSQF7CJ;i0P-;RurzpYgp^e1K=t*(j3IeO*;Gx}J?fpG|JrtTdHH^f4} zSL8eoUZw+w6GU-0PiPJ)j*e>x*9ngu=9UN+=JCW@L&%7$7qeHr*nj=6bGS6LZ8vYV zNpC+tgdVi&DE&!mL$nx&e^HnI!Lf{PcOw*a)g!6v!FiW9U4zx!K|@x4FuS=mJ9CH` zsE?dl;2e0d>bP>25shrrQXV$WCEkx=GpQW3|H#8rDd2SvTB^4!02L!eZujcl6+0@% zqP6q+Nr2m6a9JPC89YEb?{L8wAA9=J;K&buPSD{zxV66EooFU-=d&2vJB0ZlDi9?KJyAqIYRxIMb-;eFFazEU4Ay_JjQ zFQPLZjntYDKl%91i3&!Mh!_lx*glQ_(}J}Ht*A9ct45VLz*)Jo1K$Rkbx$aq&Mw8^ zie=BcCRAS2N}=X%d1L2G`(GgXok8Z~OIy9yx$0dU^wZ@$ofju91NqyH8X}DU-}75V z^Du|7#Y66$e85d;?1=$`0sfld(vNW=98_w|)+J|(R>wi@ZxjsNeqG3F5TPPDDPsY7 zUvA}m;o#DzqJif%-@cX*_Q1H zAyTx>)L+?AHT;Xvt3B%Zc&F|9|2+fPsSayhrPcfCM1M;G7zxt*;fF=lP7=hC5&$dH{ zp|Sl~znrPt|Nc$FW=v5DK`%OM6$(DmVO#1{qBJ){X7GNb zm1q?fP&mxQND_1o3!E#*uFU^pl>O(Utkp8Qtga%8GyTN)4L-ca2$K)5_YXoM!@u%` z9Aq~RG;Qk)J7S9<8FOqqHfDalyxEOKxCyuXO_u+{j~_O>n)O-umOTWov8>}T8VyiB zJaTka661HvwkfvhR_=8xL8r1E&5Jno=*@2LN?-l;DSI}Mt8;Nj%;K+)qig#A&x9WB zq$3+GKPkO@{+h5pb4HNs<3dZ@gDl1Al4uaZiBQa;u9-3$9R(9Dy&xR(zeZOkAnn>( zHO1J(e(wGMl8A85?ddvT6d*=?< z|E(cvcyYhOIGJ3jsCo-kw9|w)>{2#T6n#$>aQLB&;5+I%TRu55-uQ@G?45Pa1}VZE zL6xJLID9Rk;5`FKIz#LXE^st(%++4?Zl7qezk#LgiDv$|W{Jq^}!F zI(LSkMeBuYqYmDoUeWG$?JhIxkZ>k(j&|so1xesIXwR6};ty1v)4Dkodne-+%-7LA z=NCf!<};s|$#g6|vszs!y5PfN*v773f4-Yu;`30-qRXz+AU86l9;sq=+YWeetPf@< z-^^~bEz>tKz%Hf?Qj#ZGjp^QX7#@vxw=?uYgOI|WK9CsyMCy{VI5&sbGrhq@$zsaR z?bhW(=}z$J9-oRT-P@UcuAqZ>czi}KCX27~Fwd{En(moWI)7FoUq4Co=D}Alkx81z zMg?P7b&wE_*aOf&iER@y*hc6@*B)GJY@1KFa81`kVhWNlFQy(VVIscZb z#nVBL{;us`b&*0sTR-N%M<(=~m_KzZz4|v8RjV*W#bQ}oF%cLe;$B&6{}xOCH9g!6 zEff?<{8-&f_JStZyyHMm!{zy~(1u@&A+nfOz2xwIs%)~!CwKr(K(W831crG1vg$^; zYigM17QC=u9w)rAt^|vuO@!ZeROlync88mHLmd5O6dJa-sB3ez*rcBOnkG9W%*BWx zk=!eKC+g|{1%gym8bDg$rbz|)VTPD6cpRxEWM)qegbi3$+d^?+aZ5^^jV-g*{J0)x zm>-zSHgc36?HF5xfN}7A=pgZOI`ZWbVz5GuA5u zG40X74f8f{N;SJ|k5~{#nPR`cGb7EI zN1w*#^LEX3K@X>^ma}4|WI1BcI!$9hp3y@zep5dBXIap8&L})u-LHiDzC618p({cI z&1oJ<&bTT(e~DcqAp>0%Aso_?qi%Sj=aExNm6b_6g*|f6w?)j~Z@J;JW-#74gQK?$ zwxFnMDEVlM3$3A`fe_D=mXROnjbl7fekAshNa4P$!+}M`-aAK9dapD*E#BHc$k0-6 zN`l|6GR*avihxAaSh5%D_}#MpEyKZbv1+X7*7CL`lO}o&!z?;uX#$w1YH3ERz_~2QoM@8gSVq@BS6s| zKOJV6j5tm^u$tohfdCxfEHj+xSNK)q1`~J_!M}8fB&K+Re4GOdhXWz|=~X68t|6n$ zac4NYHtQxa-?eq)FXsSXOG)n&MHIS~HJoyI)aZa(*7KIm)O5?h+83!cl7WRq*t6zpI~-;SI@$P*o|cBhVI4a`zFht_K_(fjb6d3 zJ&I~E14pUDMoFEEh!}15mqUMdgMGD|2O?qb_e}A|7urO%)se{Ulihw)mABc{{AB}X ze;h%y%?)@NyHs>|E$$6w83lN9qmrZo%&7{ShHi0rK=9aN`Xf^RTjW-~*nS}gA=1Uf z_I{m#DL7h|7sD4DilTmk6yM>;GUtn;N{9Pttp*-mQjExKVXP7uZLfA-Lf5ZYpHS|o z32c`eQ~OvvoIOO047e}FG-h+sZDh*M!cT|iyk={w*7mC)n@u<2x+HYAdEfI<>bLan za~hJU$M1ax8+eA7zx;YvAk>jS$g^Qi9kapmBT=t3{Me}Iwum))zd7D8obrh@-{>nr-L5nCIXQK#`osfNG*+~-2M3{L? zRFaGA@XX~ZJ1Gcb;xrTsAO6>CPM!+Kg;uoN#go9hV3`t%jMY!~Hv9*cF;=0CGUli3 z>1TIJqDo#KZILExCL^Z>QO};w4~t-DeOVU#zAP z{N91g`f&@;8^rrQSZv5vYk3_&r)))#+$aRl6F6TJo>;j)K*uMU`$Tet&!VHbwZ%gW zME8i^f=IN!c{1uLvCXGdXaoz~`ftAZ&40b8@NR)srY76RfGt8c$RfqsAY+T2*r}?4 z;43paDwgzc&1M|)`WH2n4&Gqd*0r=^Nt5-KEdP8rC*Hfc+1HVY=C^6qLosbztf20e z<-Hc163O%c=eKsc-s5yW3|BNEV~&b6%K z`Ga;CA3E$N%@%$>Ws?^(w3hnzPlj@BG3~?a8;7&Mp@kkG;^1z1y$AGMR#0IiU?XP6_oa#7%boT#N z`o^FT%Yw3pXX~Qsj|3j8scoy@UO-5;@3{u6rym;-7&Q4Bmtn+Ep zLTTxtV1&fy#{rZaEgOOlv%Rq_Xck}a(Ui=gIZ&qp2QUHx!Vg;46v>2`7}{$m zjh&Ox>KApG4Z3`DHAAe9)5}wHsD^9_oxMM8|FC5o`w`|oRd7S97Mma!F>0{EOn1Eb@ePc-L5mpf2CG=9tJ=ttpxo6# zVNqrr?o9;@G96;xF4EN53MN3#Qc@)q=AQ$|@Cfip8poMbnE}rfJ7Ep>i zqyjjQP#>?at|$@g!7v&{>-eII_S$Gj)13(GPa@tH=MP<40!P?8k-J3L^A{iSXmf-W zrIWnh2c-CQ0Kca}!^3hkW@(B&9@odyMx06Y?-LLu38ZoG<;O`bZ76SHXIas0O*pV( zXxm0(2SqKW)huUD!@AYNej_9aZ1vA)QcnR3^zgc|H5B4jLN&8u1+Pa@@>Xh#QCBOM zZJhE1B#Y?ALwXiqQLMU%;7T|s-_rgkkJ_5)nQA>=%UsEy+6`-_x zGye@sgJ6hF0bWeM4BKkD*H6YYo;*bqZ0Y07&)G;?TUr=BL@gZ1=wqiVcl zHW@Zl+pzSv7X}NzV4%t*NAD!l^7;kq)Kgher+>pZg(J(|w#{cdayg=ry#;)B*ZL_j z#wKwz!hSyGf#ULTNT+(6sTAU9O~12Wdh0Mf4A%#F4H@A3hC%~;O zJ&$h_QAVZ@wX|GG%WrVX|FAPPm*wpXcTSJk1jp{GhYhL|I9j5GXWn@czmjX0_4HTE zx=35C@%3|z0403|<%3ASE?8VH%6TH>p%b;$B;?b!#`E?aYy}3!BnHlzkC4~3uO2R( zb;I`@S$krKQa=~-zV1@3=Bbi1s6$B)hKrm=yjqnb=COHsUY800paG4$b%>WY{G)t? zexcNnEH@;4an(h}7g0Ms)vVv+Rdwf8uFplX`ex2M@INFkPAS%<@hqX~zB?ksAS?y< z3!%fF);4PxthTQrYkHy7jg8{V(D9*vt2y#L=d%+SMh2pAjC+yvjESo2Sf*} zd93T_ls40fT|O}=x-xf`C~;$}>d^*Y?EH5X8W9lUhmJU+>}>Pi;Rx2(=~aM2k-^=! zCN0H28JX(WZ{}~KIubR3V~ak)tds@C@9j|_;Vp~M!R%vJ_9n3IrGNkc0DgA?3`<3; z4ZbA3?#3gipinUcXaE3YSx`sKp>;QcXAaMwk7BB{@E6r0#PpZEd53bP=sD4MvPwa$ zf*->M&0N_`HBh?!pF9MBW!+fQD<`ccjO^F(5b**;e=2n-d!0#`a9!DH|Z#ScoD|ZQ)2c|2D*jG zRe}V;i-zD?_{u^7-vc{Hr+!RNJE|uRFolvI*F+S`XCuW)g!hOIoulC5Uq4ePiq))b zI-KL_{ZScETY%&}p)6OJSWN7p7*})yu*An3(PlpppI1u@*eK7>ZZbBO1Ea-22*S}9 z#9D8Sy%P!~8}N>(0bC&ucE}QWkGb7j*_4xySED&PSSTui*Z|Ktr{z5#pV&s zK50Q->@5}~9CybDw+NR@>HF2{JzYuT13}QC?MX?osARCFr%ZKm_T_0udx8`GI0*_g-txm z&d4PI_;$iXb>We>h(pz|!Z=rF!Y34Mm?_l8svVb639_SQ(iP6tQyuyl_gHDwDa1tY zTh$Kif8QH#O;X0OQxE_foCX_L1tkFhx?|2};x2JsY~1#{f;`g@k`?K_11d#1%kTx* zPSfw3rt1ee^v@O?P^3%8WTK*7Pg^sq417i1RBtzU6S;`Sb4$sO`QuE+hQHxS zB*)jD@;!@>U9$#e#A&Iq%u@u1cmq4b-Nn3e+hLlRdVL|p`}C$t$CK<#*S>pS8f|%y zK1{b0IaIN2EG36I%IuS?;*7h-Wtm@T@y<9k#Hr77kL3jE>JB9J0A#oZ*Kn4hNP`SeCB zRBI^2xKv%zAZVBb-)tv;q@c$*;EwigZ4C5I=46+)0%5fq;q}`TV}QFM44Mt>Y7)g3 z`!4!M@!Kszbs}O#QBINdcSR*ccxv8KJ-6w!8fRO0!&Rc4msMnCd(K-`!=!1}jwM6ma%IZ3D|j^LE=M-vviUXns(r~<5pRhD-5?a2@cREL zPQib8$r$yy372Q{bK{09<=@x%yDuO?n5!43^g;^Y^g)L4nazF7cyIs!0FUW2dxfq$ zke($lBFEf`zz@1P$BBZ1bY>d-D?*<=@#Pa%)+MUg6qR3m>zCs8ppE>hiLZ7IJJsC8 zc(m-B0wQ%^2U;E$gXHdjdM!175^V|vhlpOvUX>j}kh##r6}2}qo74uGSkL-4P%K(Bq;O@agbkm2ysNzS4`{PT<9z|N)G0rQ*qrXDi6qmia<$&+x&YWd zeW;jK@(z*nR2>SNf(G_JU?h}CyNc5nzGlh zPsOhxN*J9F-)(2g&G7%j?%N{=P?#YZ*q1SAd>0V9KRLyFSawfBMbQY4t(lo2BwW5h~geFuY!1i|JE0#m0I*HWX~ROcFw z6tuF^ACdX<{83TH=1w387U0-Fk#h~?2lv^MQId{*4HA$?V70IY7Zb!LN~WTPxQbGR z&B_dyVhy9P>hKoxnybCDmrZeO$r)Hne3_A6R_It*ssiR{&_MbUX)Vz+Cu8xAKv5rJloYaLc?sd5aEO+&;C&o*SxPEA}*{uXFfx{ja(X} zlVd?uB-jk-GS9R5*Vi2fN;A}CY#dN87(GF1l}HTr%O`QBsIXZs>pQur4O<-a<}n5w zbn?^VCvX~f7}s<;6zF7rqSvbk-)TdxlBKxm1FcgbWV9w;*q2AOQ{LhFEvE`?m}?a@ zg3jbqMvSO5uYn4c*`n+SbcUTuq2}UYF$TDoWY1@5U9dYY_LLXhX|E)(0+i~Nczco{ zZ#7ssA7=;1S_i=EuK@Ph`ySnNHh}>_1SxS=(h-I3KmC*|fJot*kueR4)G(`FB8!>X zpD=S|dYj67>=z~_`xkaZ7tkKiv*jPy`+`m4Qe6!;ko~A%khmD#6Jq&<7POoR{4`xm=0&W5lhv$ z_Z?9Fcmg?OmffY=EzE@cjuk>@1O5W~M>}!!dW^P&yg0`dWK{OERR!apk_V;despLe zPYJXM;2J-+#{tRayuLus7U2HU_2y1UscnA?IUt;Ol&>LyW247@;dd7$T;S$Sa8i(~ zb*Z&h8Hj(5{h~b3lTo!y#yNJI3|)n~?9_u{>rJx(3Rp=mb~TRq-rI%A)M+VfB28 z`9I4@-PkYK#?gXQG?C7`FR1-94wX6U+l-!58-)ADt%R!ZOcx2tvC|xIxSN*@GU99~ zvVfCrwu2^dfEuz2;^?u94Nr#$^E4@+tpbMfW$*!uFm$WjSOXgAz^F0Ci8prFq}-DU zfF4jnRUJ!47k7M`o!wyBB@a2X+#~{v4Og=Q9)~XyuuBef)yYvFs@z5P)Yb^Mjg~_aySa=-*fo6?mvDdNC6%DY( zj`xUi^|oBuRc1gb{H@;hq84hXe`!?SZhI@JOI53QV*vUT>{5Fr_2WaYuB;QINT4UJ zvi)Nh6bz%txTe20%nuA(j7%9^)rfU45P?WzVlt#NcR?76O!#=EzH&HN3rHj0j!J98 z&b*oTpRNBBydZ#&@+|UPC4oH6-VwbDW5h4yC>V2i&Hv^sEV(50=&bx zb;9uLAbJ4@aPaC8v=S~cIB6&VF95PW?RD}fFGvgFkUnV@<`-9QsK>L2a6NrJ3-J_@ zc)?(r`K7YZi$q}e<(oA1nk8O3aZ8?s5Q4T$UZ zghf3+!W&1zE7iW53kr#0^Bp&?bScC9M6HGS{ZDYOrX$>2`8((2)K-!NU$axfpQb}( z_|=6F>I{iZFL?M2K=v1RCvkh;+BqC)M%3Vh+GS}UFq@uB!jL7*oiM7ei&&kgk9R%9 zgfu|3U;J!Y)8_0@sz49Y?%jU5;M5C7=L&k>syu9>nVq5H3IML6hdRXTB+}a`H?QM}_0t%(0ipi8>?@HHXaVkJwyrSr!;? zPwe!&nUX=#NnvgB*oJRl485J7N?(3jQ@mJgoIKx^6wn7tVaKj)!iL;LdV(271m8EL z=A)ZWErpqgRKPuR!~%%ft26Ffx`qUg?GTOdl zZJ@tH6#{9ItYGfFHT0dEs{Tn)yZTS`J);s^-)1CYr!eK3pnE6Kfv1Is*(JLEKeFdXuW4jv!q>rOM)Q(J zq3&7BlD>4o6VBrw+f;cC9fBM_#%}o+IpTlEWaCrMhT3(^ImOI4+1$Ghl_?29u4ARuRMn!BR}7jdO(HWd;pf6^G)>D$g^xY1{T?Qt z;WP*8^s?#O^C)B4Fhz~l(V47ACyUHAxnAY14Vj`7yi-;O-M?=g79QIYkj;aaPf~se z%u3~T$Ur(RX(iSn7sW?HZa$Mr)yU3Qla6v2#3szEPg?9jj5~?d51kUoimRLY*sb9p z`yKVI02)@2iIeM1QT(S65>b<}NHzgW{BI7R*rcP@ag)W`pYAyaj4MowS)--+!=FR8rUJ^0_kG3}=hI>b$p zH|G4~j2^8flm%bH(BnpfHZmthGH!QHh!fmI{&R>qVZo@R^=2Mz?;JsA;;ny)mCROMFBl?u4 z^!|OQkyhu!4NQQ;evs^^5Upy{F@7>@FhD;nGTgm$db!>RZ^y8fNE+2ZS$B&DF9DqY zz8{X+om@OkHTUap7uNq*c;(!2ds?2s;lcG_owR=>RG6t!hkf217}M;Ymfuk&4>;Cm zlrN|)vBuZ-rI6*ET=2d}A4eWjoAjB~gjdfPtOr42$`A>+NOp|igx z#Ci)%-2X}aEVw?*8v41O=+ouR3Xe%VyRZNN0lQv7DX(w@Sic5Kb&kT|0j`gxK7((N zAu6pBLV7mMWbGRZmcXrDXphOKr243(-zwwrjU9S`noV$Tjd*{}yVdLE{yBt0^3LU< z7{RR~l9Ffl?m*q>N)MFamx&p|s#XHGABfQUuw!9(Kk|SZ#P`4@qFvZuUiFO*K*u@Z zX@-36xVKFzyfKUDb|fXRA%*H`R#RDh_mo2b?00~!27Z)G_6aWFm{%TxG z2ds4rP=wnlTe_DLJ%K8BQHG!Z0S36F;$(k?kS$*~ckUbaSpD7YqOZ z008noYE_eKx@$(?vTh_igoyl?%|IH6(5iYvQMt3~BJXP30&i+G%{Fw$Z<>W*I^_1t zpBP1M-11ljl)&Wgb-1gYn*Vtf%^JXUCa&+apLdPQ3&g#X#~b7Ahkt?q0001I)3p1# z&YC02vqUXB0Kndwk%nirrLYr1nX8L)HXE?u++FRAIZM>hK|M`^suH7dqcxq@I?g-@ZMHBq?B~A>)pRxlP?=T^+Bu~M3L6J8b z*K}@rBBb5+$GYR2Y&jiX z_RUB{FMKi5T8YTyj*4|d6AI9%YBV~e{G@AXCQ`S&iw@V5!An#1)NC3hC7dB zLktbVwxd6FDLU_&SdG!q2?wWdz-JV?nr5 z!c@{(at2qqnM(b?k_3SLPN&m+2lx|g6Mt=QWOIZoz*~qhnvHV>_t)oQKi}Aw)6aXm zo~`TZ$~GnbAL>W}cLb#fe$Qc>(5Df%GM&_uZy@1i=U1%|*_nm=ynAB($nYx}^dHCo z7oBsSbu!U~lE=l@$fq4GAMoIg7*T3Iv=8&IvPKWX4pPLb^1)bta4%dZRo)YD;)Z;0XE5`%I!Gtj|GL}# ztM#NG?5e!O(#6VqDaJ;6oe{5Dseg&^@vIIVchQ5KLk2#y2sr?TVT@Z|y5v*G4c}t{ z#s$ax$=1~!cFpE9RKTg~z~{Vi|L{n_J5Jali_F(?%+2cqDQ7kQRd5r=Psaq3 zpMl8G15?L4dZe6ddmY?w@OaO)@|RxY=wJoWlh1(6=tI%s6_g~9)$45s%Qw#Pqaj=TWaFC0~GMmC9L`qrO~hZi2& zIwlEUL4ge`@5Cfp$asHfF??}G#aJ-ZLdE$G>Tb2uR7rMJQ3 zJm@PN5{HVne+~H4WylpBC`04(!j6Vv29h%>Z0AQ1gO@<`u^Dhpln3cg>zMGbgvTd= z-LrbCWZal$l0iWPQCO*%#EIqJp($a2BzY5(?-UPRZgEw1>*zG;9YO2^K$c^lheg>OI@7sdK0ngL-P6wI6g zeIDgWQvr?pB^uyD_{LNr8%F51KTyM;@}~e>IEoX!wL%lF^r8iPcch9y0lvJ$yCOM_ z^Qoj8Y-YM-t_@ohF`%+o+8Gz?g8xE%f3G|ok)9(ahe6-r74{H*UH=JKp01_;U9Pj% z48kiJ{sz75htC)@df)%4TY0jPpL2Y7v0|Oj?_dr`9~J84VdC_E{GHv-FAN4#ZO(;b z@qk7BtQIl=vJhKVqPH1R<0tm$p%DC_gm7_FIeh7>K#fzQ9JRPz)bT)C%BL{YhTzyy zzPP~zkuVD8h@5M4?4&JpxgA`_0>&%@x?gQSl7`5nw(|Y%h5=N!SHOTQ9t0j>x#Y@E zwKuZP3?o5fz7t4d-F2=c`kPiOW68OBT=;C@oGGe?U(lMruO~ zm3X|B*bDZ!QSW%$qtielZ>uQ3P8WRO_J z_7}|hXoYb!141z6!o#V&p35z;Y*lX0`}OIlD|?h#|2D@3?wsTUW!pVcpK-LHQ#AY}2#Xgmn{ERPm^)7nsJX zB4@X7_BxQ9(dAgW|56hoMD}t8yIlW$N3DZ-PB|eRHz#RQ4SL250e>hzvG;ZgUHaxf z$Khz;lt8Vr>Q~&hu(+6uPhg!i%XBs;=+oPP)-e#u48-aGKCV7LA53bymI8_t--0;L z1BdAqq>0Y|XNinGWB`^#9vV0w$ssF6-`S2)04=Til@-x~6B11ELA zvPnO1x-c&dI4Sf?FA_c*aoV8@Nh40ieZ5rbfBq_;wkXku+TDX@ATRgrkA{-gH`eF< zdeHCNT5hb2;L_=2y0SzKIHYT!850twNsMsAiQ47ttQo=>wV8~2{{y$R1-Ix_Vqi_1 z{Gn~ubT;hsy`@0`Xn1;%U_xzDu0g?x2nbznSXZzr84Q~v*ozJNe|ghKthS%P5m&uZ zTRr0ymFV1zxnY^;3B;K*#qBqR@vCiaNqu!I`3gfRv|$q_+`AQ+JHYQz8A8u7Mu z`W*DB%B-QqckmDq>qIi?yS0!Z4ZaoNL?no`o=bE1cifq!l2Sua`TtI>jqmh5^#tJI z;s^?Z^L2WBt}OD1s`PjV?-I!Le4xS#5;pS0vaPMASI7VW02i@XmM~WGI#+N%G?D9t zWr0Xw1tMM*d!o-hz?MwZnM*wigaLlQp#ip@rBu)v%(bSh;QEUwDb;FtdppbROLJfv1yJ*^E&Q8$HHJXOZydM zJnu!jLB{5Nr~E37U=n7GuQ%4W<^f)&Y#hNoZdl~ERi0;ub@v@UYj#)TR2Xlb$j?UOMR15rNEc64?*X~{v`vb0wKGrD?AN0c+o-Q zXeW&@6R{Pd0td>k_4P|2FAFvbG|W5}ffm(i8xCJ1IzAD5zsW}()vFBX^+}}WYdD`P z)`Q}=wj7#YddSqP*rUAut`h(QNKspAL;(hL0VlxgS7E2pn3J-t{6GWACpu*_zr=A* zxew!_H?xH#2SLb3PyxLsZt>^OFcQ;uTP)p{`Jos900005?<#7IVy*M~bO9w8=Cgg$ zraSyLByCt;-&CQdZTiLoie6@Xlo>({ryD$8fOYJWVFA4w+Kpn;g>efI%DB6Vxy6UP z;22q8AZUQ0Kl^qAoaNfH7w`gX`-(Qt5R8{EF`%1xEKlIJ-L(U*j?cuI;w3Gc#O7uX z(=x_P_Wv?k9~^hUU!PW%@ZH*pnyzdaS|`{FW$(S3`4;Xa)Xul{2+6bVhUmLsV_NZY zJZ*gZp_yxtaCRVlc4NgABg)8h}(l##{B*k(G`yOM|^>!BI`V31J_mc8Wd&x(!LR1Xi;(2uT0beH}YoqklIu?W~Sp`q1t%Y9$S!WZ z`h)I3cjp-J2#D0vwtI6j3cOU9HtPvRyJcgP;s+S*3cc_!E##k|b)Z!%OdD>%ksI}B zW|0$p_m0|!xIqjf$Rpg7w(8{*nS))TK2PMQ=&@vf6-q8iXLQLLnQHI7k{-14-;-|F znUvLM%Rd^OjqEIh?#Fh2ktwb=m~^}gp;nT zc50$=(e7+=ySQH^jA4Zwe%U%jCM18s5JAK=Ga(d&yj6iO&T^4(@5F314JYNs=S)&d zS#a?1z!Wc1r-Q|!7j>#8fc-#W-5Jl;y94p;weM*QT)HW@Bb?%SuJ3OQCm|Xz0m;pn zIw}gAw&@@md2Ln-CLbqf2C}3U8pc!}j!Y=aLQ(PYb?yUF7Y#YT;%TVDuPwlpct>BT zoS8jmhD^RE4vHx-x4~tJ1Y6(5X#cB=kuI#Y=*{Km$}6kz;F?uPB$*TK4<6_Y&4p*2n|l^RLshAi#-O7-1Q-dS?1 z$LH7V8XWF+yqG92(==@LVGF|v0{VKhFy-szdx22NR0kiWglGky@--08|CIOe}J z7x9jce&5(a*t-6oLCnmO35T)O8|?}J0(!Kzqcu) zbjSYtYD+U12DwG*j7@p9_Pw-tONq48VnD9-lXmGRPcZCs=Qz~7PTq`9PV<{hC6MLI~i$pc)qV)P6%uaX1mFWIrlpa=sYu$Ww&0YADXk#<%kEzRon{ z*v8?GQ*S8NEhp7o+;8gxwkHBiSg%a8b>82>yVCeeZ~qO|v|PAh98pv`3brTH%`TC6 zHy)*AQiWd>gKH*B_yoHk)#`E~;GZCigl-AJG2V&mYENw7MPxjGlc?~CjOc4zh`g$q z*E{W=1x*6u0f}2?7C10DiMt|3)K-UTRsDu02DA*~vtrj(Rb+;MeVX+9>*xWdgkspM zZ@~Ir`?o|k7w{S76XGU7O@xXz2D&PTFKAVm$ngb!Yg7q=$Ih#IT6{1bD&*fvbBfmy z62SEYxy}lnzEbDe)U-u2HES|0?DU3z?mYSZ%~0@m*&8;F<5T4g!U0eI) zDtFZUZ#V=qZ4KoI|g535`?9 z5#)u~^S|pB+Jy0F>FoqdAr_e|6%N&DXo&dvhqs!NJz7Zx;>)O5{$*b8^Yh%D*!w<= z*W3LZqM*}N*Gz2C&U^^PU%0cT_$gNa0*gA-`1sJYP!$P!b)}b21v{=BOy5NFHpjgz zMRdp|l7AmQHScjS5mR(2chtqB>cxLRHxRirb}{|QABTu4UYk@YvebAu(aAK#<}xe8 zNMh4tMuK!)P^Cgq?4rS&|9e~7Tt5k@=f%z&)JSXT!(~u%6-FH@`HL8d{@wY(^wjQ0 z@rz?=L&DTMK6H|$L1o!|qRI(14l0gDnzF(^r|8ra2Q`zj4DZ3OO`Un`x}LKqZG%)z z``jdW(qo1xvOi^x(yc8bn&*#ZG3%X0-8HgYTKCQRy?5%Au-j{G3?pt!|1XLx71g9$ zj_;lq<+-u(DY_R(m29YLI%q=C(8sxKHvzs)j`V%A2!$>gG2vB$no;~UMx(Ken-UkE zL&DC;EfYtLGB!!orN-pXSvq}7#d3d8cp)OoB_}n zZ(T}DI~4L+j%292P*c&L>|sDxG8#2$7yq5#lKudGKzXT@KCAz7Ae=?VKv($=F7Km6 z`3A#w82*R*9Q#j4X1MYi*JJwsfwOJj4`P058=e3~43{3Ce;?mGYF z`64gQn~gho2F=&I2~tCaF5xe&LktnrCzCcZQ=%J+$8Vp2EU~BKR%6!NZ%2=E`wC62P33t?AeaVf48q;qeCJsk#4~RZ)Pzgrl z4u|s^rb<%eL-*j|>1NnM425AP@9?Lodn-`d^6g5gmP z57TeQCPB^2te#O8ZvkCM4!ERSXTbnlk7D8!YXnNEu*T-bs^0 z1CSb4qkq65dNntPuv51rZ1`4>hMJ(>Hx(B_ywYbTFKfYO=H6Vrvn6inuv%%xIARa~ z?n&^cQ5k03t);KZPf0~G>QpOtOV59Q=NQTcFe`$T58)bP2Kp}$%Vs`~T_ zr45sP5+9fVPjZRP`{PY5*}o0_x_7^S_R$2~vWQ534wj=6pu-t>6SOPu~SZkxl+b_XBgdySUAi*Kda z*627UC~$DgkUUOM4lU$Oy_dw?RV+&xF9tgZIxq879x7>{!@pq)+_pSQ!(L82cjf<2 z_y?+7w0nGTLB)jbo=|t-4<(=-2Z$xcQK~;ENy*EA2hO8kuJ*r!)uerN6>_pg(`=Muj<0E6evX>X z()pX1x;0KSSG!0v+MPz*0R?Yp9VhQ`UUzu}#;@3RANs0g)7Vzq4>6)1`;ofQ*#}n$Tkc)2zR9#jHxR1l|NlE~}?d>}n&=4jdJku`L!2hc+xziZFyQgKo^~(eNblsoUv-L{tXn5)8U-agPCWr*~?UM$+u;$0$IJ~wRFqQ zmM1okE$k3;Cwn1O%5x`ZAdaucX^6uM##Q9@<5+a8G7s4#$gy7sr*ZllpBc?H=MZ@bG+D=CWVF#3Y0Xin&i&)L(6D-D5 zP=?LQ?)r9uyfR+pV(1O0%aPZ@?I4AsA92B$TmuJ5;NBZ}bHFjzdk-}ku=_U~ zRHB7dxwnO6+NCuuu&Dv$_n@$tD&)oGd|v{9deSsr9d5QfGmeSrjvXpo7ZrU{8pTMX zPh-B2?=0b~o3(7d7nFsR+Y19do^H|VY7z=swN(M_>MJEVGE`dOjD7nrS|6Fc+Q%Z&2VB|};<590 z9yFd)FY;SRLF7m>fPNIudH6awkL~C?PQfuxP|+ayqz;n-MRjflvN2kar>p(QS66}> zN*_39QQQu{C$Y&IPhQ92xT@re{>! zQex2D>?ZTo--F0BiXb&;XF*qtgY;v6^qcAyM4PXsycT#4x&{;$h`@HGHO<0PHym!7 z*cjE#?MRBd6nZKfiwDKGi!Vsgh2|Gh(9Ce<{=VghE)O1#7%nb0d;lHA)zC#%eEx%m&W63@%o)tkiwQs7;Kw6FKG?lA({xQ}dbv=vt*UC5t6EBR1G~?6J z-JA=l6B#^M?;`rNdcMr+jgHBEj2y6#NyRfRw<~SRqTbYICPl&PpHk#wCTrtpo?DINyKqs1GJcZXH%Y$sUyd){+LCJeGPMWM6;zU#whAw9xIvjwdNq7 zQMf_-l{d>StVh$x8BrNEh;jb(5R*q?ALE-B4$SFwGyLw@tX)@>KW~bvmfSG|Fs$Ir*=r{N=wXY93Pms_knZRooVW?tRX zhshT89^%mD`6_z7kmD6Zj0G&Xf@e_5>$CMTH6KU&)W59ltj8zH1595|Rv{|%B$Kr< ztZQ}OIL1OCKtR9*tytuS)nL}5ZLpel#sgHOBwoFxZ>H8X)-O|a@gnT;lE=G|gqIF; z$%*3X4ynGTE3BSs#&+D@X%y#g;(?HVTR8~*CD@}<0lMAo9Dx?30jiNXzPJeYSYv_h z{05MPfSSzKG9G^Ene7XR&FIuPMpm%~eqMNV)000IbMt)vsN~M6A`kJsbUfQ7DAQ&RUHqz})a+Cq)E2H31TF$W5 zRD&qj?9%aiZ(5w@A~+ZN&zS~T^hptKN7EpT^V>11bB}cDu_p65ls4CC#c!W<^6XGK zP{xO|#gr3F8x~pw7UzKu%ZGnt=+Nn71Q_RxT+tKLv+sPH%m_wYrN3RRFs!OF6rz#BkpY(2Mt}Bdf9q@io zwu9#}$O?k7dRn!v(IvFfZ+Y~Cc6pn^8#@~kL>^=PZ z7SAD2FxawYPrj)NNlme~@Z$p1tDqmESZ-L68=0!GOZkXn`RGwvU~E=V9A$AqY1iu~ z(~?iUzpS`t9ah59Fsx}VT2T%CTau0_`3lwg;8&2hk;=GVLdfs2R`-`>v|k-s8`o4* ztDPnPGe>+gX$>>DPc%WKwz?{|#Qt$%*`J-=VWR6>Ydp4`j{o&jmUzdjshO#&Q98u~#SWX2-Q*zX1{` z>Pz=I{v5ATHs`Al|3yy+B=^+UP`{_p^7XZ8KF1)9ka?r3M7;|yt-5Oyqz&r4dt~kU zU1VM&$Dn9<=g7Ek{!||QG^gZehT3O=F15`wei-u7F?+4xZ`hHVbH=FQfO00=X0VMs ztVmt{G1~wOG}9kk{LzYAf&p8?%S8;i<~t-#agd}^M5;#^(FMx>c6Wn#T?gf;50Zuh z#Gxvj8zpbmf6TGI5XtKfQ=;vUHub!>Ui^@v8gj zcsNw)vD$_We5r0-W5P-9zoe(0V2Jm0Gt9fFfGnR@MZnB?EPP3Hk)e;>JLkabf(3V= zdYGnO!eVKy7@yE|G`i>l1S(UB0Q~X&gkE(Y_-X#l+J%#9ovPb_bZTbS@&C>K!!4}y zXrH31GX&2x=Nm?FFBeOjolAax$mMN>Ci(=F#0v@p1|=9ebV^u13>~nA$6B(7HxVI5 zsf-D+_b99b@vzOD{oUthll|stq%tDjyAJ1(9>38254MMjIJQXBg_iMp4 zxR0&^5RI7^UK?a;D6eo(YVnhVfdGR*e7~>-H-9OZlS!xe@T=Fn=8Vp1ew`fFI|j@# zk5g@h)Z^OSUgpbHJs}i3b&@v;kM2S%0HG@H`bmB9m#2c;s9fVwefgkD1TvVJ)HOFQ zlve-szXmV5$@~8bv%g5DO3KH(g|&U?M7FN{t|r}>zJ)txtGwmOiw~M!Hd5TOzduBj zgug?dMfB#{_*; zp^#)BCQ$=?(&uB_FT6(30$&BvhG+K5BHk&%xzzH`Bop!vOZWXBtThH8QMk$Gsqm682$R@%eXNjZ?oXO^6=M&(MXd zz2<|>uDnML)&y)&YQ+ING7&HMWE5A1i*ej+3%ywDBemtZaI6 zT5AZbrq-$andl(^xb}suL_r|woNK7_6)Jrc-k$$#&pwxrkb*(@8G#mAgyyk-NeW|= z-St4v561^<+7+Ih_P zG7Wt7%H!0yAdeKt#w(a@V4h@Ui2Ogk@|^mtWc zaRIi9Z-oB|GNYDXZ@IGM(>wdi85aWK^aI}rt!m<*JUIkKSoSBNkX8(M-7KsEj7TyVsRXb$$zRc~U?6!#@C)e-Q=Vh8x+wnIJ{KpkLM7QHZ z%xKxgt&Q)ky>CHwq4|~?Kaa02X$RP#Z2)C@9u)Pm^L+SMe;N$D`mi0`1)?ADe4dLz z7Rsf$MPC}nmgw;822sS-fb6<@|8EytL~|xux4K< z)V=4q6q*8^H@iZ_HZR^~9rNrCffrdG``!plme^%cVd!Sd4gqX)H+|o4=A4NpKN)=? z?i<>heJ$Q$)%BAA2(#~QJYNfnt_%KqY(kO#{H>4#`M>muQu`7ZRiNL!s zu(WME(vL_~bOYn66Ish(%6~ffM9PliL&VUJ)~}4DhcQ|A@^de-*1T-M!QPr+caGv3MM#k;P zp(Y)839iSN6m}FTs0fN3_-rze<#*q7na1S4F4-+b)sZWN_t(}Bhge+g1TJj+*|tk+ znMTlk#Cgk;xb4~~O|b3SC{6DZ+%NaXZJmm)XD<{lu4-N&zBkuzB+7|e6j;?e>{%IUKylM?S75G@sQ5Py(jniu#_3lf zfpdfif&}KVGpOo-gtSiR(u?rRkWCh*+Xc1S`E=tHdBSro8u;ODD>|}4AkynmvB&sm zWL!d}HVD)1X1^|3X)Db>ICS>(!)F{&C+s=_A*$DWTQ-=4@jx7)GI<89!b zNwqyScLUqm=1H&fBp-}$UL>Ao(<BZ!q)2I zlQy5O->RH2=1i=w^ew7~2MrFW6;Xq%_Vz#6Ygku8l__UmL}xagRTR@nPqX>r$bLlu zU4~?3Jwn&F6~ze&ytB)ogX$vpzyH1O80AC^;PlA@Ld3U~dN*kb@Q!#xKG2zCAVJHg znLmReB64bSH&qD=J#V|_C~7(K;D_g_OEhE0Y+m2l%vTnIgD5+~{eDa4+A+=OTrV?p=quBHFf`FD+vETM z000MHRyKnw5w7dB$k055#U?w(@LOg9Dv^<{NQuct6Xw*A;(>P>@2%3i9;Sk+2VfDI z4%#6}xvS*|EWB5xf*OwZ;g6TV&)vt22&4cc1nS1xiRa9nyqO38Ux4O`dj^i644VQ_ zKOIhTJ@RKmdmrUw_vH^M<;J&IKd!nu%HndO#Up$>u(6PBUOk+!VOH1f#OMEL@|EpQ za@BGoW{u;1a!r2jouk4Y>6%5%a11>E@~qY&k;bECDb zhO8wOU9OS*yF?~58R{CfA2ZaoY~>0%X5j2BjT`PvHMOYjNy0f2V9bntG;2gp;uUR26-j#U;LBhH z+U&sqF)s3O2-Ht8HJd`@&Gp@L3sy138FUF7B!X#b{Oo7YIo&%FQfi3D%170K>&Z);ndGqM1rv4+-j2(poB3bt%LI?5*fr zHNNO}KQW0?_u#b4)y>QwDx_3V%T)N2tiqFQ_p)or`(!5ooxPm7sNSe}Vz!jc_SS@y zFJQ~?{8W#LE!$}P(gq&HzLgdJvQhvYYt!nwl2~?>RLbX@j{2$LyPGt^zV-CF4`OMi zdo5+i(~yyogTpG9Xm26Z6*7RwCQr4T=Z0E*@~$<^WQ8$;Q)k@XVW6ufpnyBcU#oR|%$e$eb~tXHWW|ZzRqOMExHJMtRhY> zy>~j~r6P)y&txpvCXI*6KX4-J)*R}wV0KYj|8xNH7-r=E?Rfr5kAIjihq#2YV+HP`Dimgf zoL0JctRO(8bVv`{tcx7Hp43CX_R|${(*qmI)Tu3$?!EK44t&GUN%9AldVH=vZs>|@ z;oo{%lrKPRyo{P*uS)?Qb3??D@It;!f%NoF7zzbU5wZ8OGIy?F`*H4>z7GPeY_aJc z9QH<0tmqMgS76FtYsP#$CjepYGC*hIMg`nd0dfv0g>#_i60}=ugycOyh!G)}d0V*9 z3aP+NSq8)0M(76+5!bU5G!SVte*Ra0iq#W&FCcgS(TG&iGj(%6xKZb4v+)!R!zrf< zMRb#!QEBTSSan#BbB)1{+N7=AAO-@-T5?(eWP+W5OAE4dmdk3L%5J;x!hCk2t~Qut zIMY8`y0ZD+`?;!T)|J6>UpNO=O~G~-U$lIE!6|GsEhzj8ZLu@v%ASZ$NF-O`#Yo6b z#%(pvq#*uC5h55{%`IQmZjoI3*4OIK$qoz_q1m4*q z$XuJ!(T8xzik>lNsTI0{LC5`ZMLhqu_HHW=ioHc!=I=Dpu~#w@vqiO(kf*6u;DkRm zJMImec)fzuJV$3d&zH}Qubp()b9qUPUP5c|cvsZ6wtoQ_s1u+AixuEUc&GIN6S$emhF{UFM4uiQpz;yEC<~ zpR@2sPq)^Kp46-ck_^<$L)^Znh9U{+ha3lv&f}CzVy3W@z0-6B|QJ z#!S%;47^J=HJ{w?%OGH+SASYjQNUi@&slJ=DmlMgupJ>KaDK*w<0&NOc|(CTO%MK0 zj6sAD*EZZfEy^1R1*K%NB2ul(P^MkTD)yrI1R1Kgr$*=c|6%4g6q9Jg>R_GPD1Zo@ zoAyU5Ngn4j$kVq0*Lj<0urwkY9_z3Y32Ul-R|MeAE$`Q%QsYUmpRsXTE*s z%~bqc^t584%WQe+C(o&gNf-fJ>*afnN#eA&to6|S1wkA(6vlR9&Q(6X8H%@s%65nK#zq?w+oQ61>Tbw40QnqC)VJy{C zJ)F=1=?ALNX1=^~l2$}~9@YmimulK_UGgugxI;{-K)wQk72sD7aAOtx6I}oR^$pYO z{*#HpBb$zuo#^m@Qf_2?Y>t?M?;Qp;Qp~<^h@qD~5;HBf`cWA)D|u7>>v?GIIwb?ZBAKT^I(a!%JxvU#m6+aTF)CqS$x7y(B( zYjKrRqgeG>m&`Omrsn);rROkur_uD3TR9onG+os1?YW+&ko*Hig0}Wr#>eTlkP)-{ zNtWt}Jnk9G0N=e+sb@phS&#q#1;AXSNjzRIUjwvv9EOJ#F#BrQ4bN)sl_f~BG=Rw; zVl8GNzSfccoYDs!K>3t?OM2fhDevwGa%^D|z`VZnQ|ISG62vi9pnAsuyj{HGwU$r# zB4&axauOdXsW46}Wx?hh=maV9dKLqea%JC*DS!8}w1o^W)(0fhELPLZ>Mr}akHO~_jvPEof%7Q5I^Z^|+d z6?JIWxhq;w>$c}_@s?zND*if~5EBTzL;gIlbrV?XRuHRi{1w}4XF^X`Q&teI)ylp(N5A9FvK6Id;Pqp&DEevZ#h{Y=Fs*S& z0jbGr20z8)1T(QZ2s@6(iab99PpV`*--DcWB~D+a&6Vi1rL^EK-#(vzKnXHj^QM0) zW4tAGp{Rt8$HO7nXFpd$;clu?9 z26@V}Nk;RspCvO?FXNzWwmM3Dw&;ZtQpspvmlZJ|#xH2I$Et7>CAVK^516_dkH^jy z>PisQ{D+DBStP3?V~rEvhM4f+^h49Yt#+UMiJd5V-hLnYWFDkMd6dcakPedy?#QA{ z6f=n;z;*Qu7a4UL68Mm*D%DW=*x%2N0;}j{zE+al?!?P;d3q7bfN=~8;(=~)l%UsY z;z0tFx(^aUiB{`Spoj~2-f(wki;XEwAs6BGs05qCs1SMXI<|N8{(OqU!6XuemZ-}9 z;vO*}*2yhuVE;qi7aTh6-8982C2Fbt+S5+xqjfc$?b{eD9hzAZ^w<)5w2tTH!EW$P z`?g_ra98OqnT~ygTKK9K`+b)i$_VpV`BK2>bkAn#G~fUMI|9cGYVvET8$Av1PAu0` zx0>UyLV<1+SLH_9EI)A4viO#y?7L=|5RuG(WV;9<<&PK0&&REmk8G?$XiF>$cB6)~ z;zmT`JC+o0|Als5`4U-V>gRg-?r-ZQ=9cfSxNGi^2HiWKq-A-oNvBM)N|c z>nl4pmZ<7L&YCvrH)}H$&Gp?>p}J;JHg_P3W+n_=z9fd^o67DhU-#~;FY=HQZobdr z$vg*pH7SsAE~TvPOat#RHvNo$%^%S|cTR79^aEDuPIuDet=7|b694ayB9TZ_#%HQ_$xZTmw!BV*3i_P*L#uQkKQ z#6m|8CK}}5?~sT#r7{OV00eaR0&V`plxg1R;wIwSV6>>Wh(p>Dk=yCgCT9)G$`k2# z=lOlIAzS7xvR~lD369hoL~Sja99C<5P}AFsU&bM`E$vXuaZJMr(f;fE}yM*XXUVgJ^HPs zboaDvDQwCy>JPUpU}{b1f9D~3TV$44ztz@W5%M%qIQ$>`Xk??fou_U|#^ROqaQ%`C zOben4Egpz<*}zZkD}6P=1DFe6nOK$toUb{Gjgrjf@@m@G`IBlNsW?AY{OATq_dfPxg#R8SaLwunQ>?_;ofTIb&g>9Kpz~ zpT2d;YEz-Ycki7W{t_2%g3h-qKmf%&;erunKA1B*S)9R9fRnt+LQ56vAncwllsEwS z&neyLqXRI@aM7k1;dj+w@`OfqKLbS5=Y>qViAU%-ZuZrIOT|_j@>s~7-AcA(rD??I zM%HS|zRQ9BG&lYe2J|Q*g>vUK^MGTEbTC23Nmj`WoT~BDGoOl533z*7-X$vUS z6)dpSt)srUfB?b4ejmxL%wvgu$6bDQW+j^8@P><*wEuYC5advJ_Ys5Dv*R+kczVEM zIgkv?9W7F}N7w^GiR~c`ElRE;b6dT~(Lf09;lUIzTE;C9`j0l+6}GHW;>XSc#O13Z z!s^h(Mb|ygiN}Pd{F7-XMmphP$h)ADU=)Y=F~$zAc{VS7xn$-y#Ug~LRxnBcif8Un zmw#U@);J(W^tO0Dk$Um6RGd9=A8Urdv|D5Z)|R@24rx;j=zeR7qM?1V#u@uJBFaKL zOe%lpFX6&{5FFGTgzxNC%pwA=scI4%-2ear_#>TW5Uo*`HUf8Tw&8|Jv~4xuHeYFd z%rIpv1e!%>cTu6uQ#zLsStO}69pkdy$b^|>3k28CJ0MNs_Z_UY&Rhrx_fY-5Me!Y{ zWs#>@#e5{xnw99*!68~Rsx(}vafhv%#0)ckp`M z{qk4gOvwY%IMa7qi3z6yB@VUQh*ErxJ%U`h%XMrN$dVn{XcHS$t4%{j;?7|7Q89}L zV+R1#41>{()I+o#rqEg@(QNj?{a@lsygHs>>Bv#AsTKGr^x@_*`XIJviOLv2RQO*< zizEz}3LYcTd?7Pj-A-5I;=p@9BZBQzA8WS6_oh?~^*4O~`9I7bL54D;A=U`9hdp_d0`B&R4>ozbMHb4QgHqDe>CVY z=p*v(Hm0MN)#i)c48CPJhX!i^00000006c4W99HWM{$EdyHh!4p|^$-bj(Ir_?*k8 z=_OehOmTBx7{4#%PAk*<x#H%jTmG@S*ASX0 zUwQq5fylT7BUk)>Y;_Mw4R_HrBeP|isD4-&;B93MQ^$TO)Uq)u4pluwV=^K@8J3jJ zG01!0P~gN+sUXrZ*{agE8c&N7xH|V*!<~|Q4j72c1C91Zn#`ZXY6dNg0_QL4#iUyQ*P0I@Q4Kd ze`F*M-T<*e@9#ufsn`>fVBZ*Bi;^B@N;pJ-rGA?&X$q&%#%3Lwb<}&Ojz)GPJ@dVm zoPz0GZCN}Q`HM}#4zHBaqM`9K^#Adn!YQ}rSX&N1;CoYj_OIDznCT_|KqZc~nz!{- zn?L_%q;Q}skG!(^{f~YNur>aBEjQ=RsGv9jR;@wzF zG>zy7YcvazyF7Z+!gGJ^b<5FEB(_*0+kRRC$u;=wdB2z1-f{!)#Es9l^M!MWvz5Kk z%1@aEsS|3dvMV`RpLM@I=X@-V*VO&xS)#g%lhD5$?(P?WPSii`H(7r&gf<$0kT-}F zq1v>DOAqgdrOaVP*#ej0+JC#dE+m@2L|+AVr4zX_NhiqXo+a;wy}}?F&D{~@p-&(_ zGm&=~ReVgo1VdhunO)0RBsw$RuYYLg9K7z)X{yZbaoR?rofU3486(5zS_?1wLMM6Z zRLeo((0#oA$T&qp>DQYrKr?@O2pR|(+;apxdgb0(NIzryofGvE{sZk$+Lro!y##MJ zno%y#nIPSbxnf$Q%`{Xr634}H{x;#iPj}kP%3$9%Q$z$gKcy zVV)FjpMVp-pN<93*7q>qt3PxqEwE|Rnx}kP?%j;x%Wa72Sim@%q4{xdfksVOi8&4d zae5or$bS0hTbF5A|f1ecR_DMnER34^k}AcUtp#Hs)o|2>Be5VIC$5>B34rxqruzv5fneF3s%= zE-P#Ac^* zIaiPX2DzAF`Pz|FrqlM6RJeMq_7mNp(=}_*H9sroHXaCvsO$}=#Lp-CN6d_q0rJ_) znN9pPbu91*9txwX$`o_1R5G5TMN;uz3E%8Vl-rD<6q=eA27h72me-HE_TY7{e|$eZ z6Ev~IDlgiks&uJ<7Nhofj!}X4zxgRy(gwacTJae`DvhmvJ8tQUFE{uhd!<~yBIM+U zby_E;+X8xdw=$1CMeSpCfZO@OU;347?{3XRZXHTfHeqc2d>!{_CAMTf4x39~SB0;; zGS_ctkB>LD?lFy2Z~>e~o!z;_?#Ha0ehG~JIj@JbFj!UNc5hW{+C{)~-Ak?0Zb~xI z)*!?pSgPW)w)~pD%OI~SjA6W}wcKE#iW(v$&cE1a-IBVjt-T|ikkV={LLtfb=>b7& zCf&z-TU`1Zda8+E?TIRW>pCXT6qXop_hdI3W!}!oeD^PtV0cFOUtL$w-S^bL43^WK zCqJ-QfCJ}YHt&WEY5|HQe9ow9kTp>}$!i@&j;W5jKI471zYwIu09xt(Wn8MPLA#7l zLo)3=x%Xh?Fv*vVaL;9@IS`{JxKs9dPZdjp0%E-;mySFih=!YQ0S*~MqVKVUPo5e+ zIbT^v;9Na2gaZbFo29!dE5|^>0el*^@QB1`>s$nmOc!yQ3v%nPd4_>U)qns14)(9DI3^C0zyj|VIA7q=B&4J- z2{}wM#o%Hx`v?eJkA6H{v^6;oq3a1e;E;)LRD~jI+eF1XH_~?v;r_i18wgWtR7*oH z(M~mK58>krs(Hb#p53Xi`2NIiAxj*NPiou=WslRNyjuvmU3jvxpr2L#L=`%l z2GSC`Of9r~g{t39A(M#e<(P;UX}NZ#c=qEi#n*T7ujf?e8T#e@n2wY2rrwKs$h*aJ z!gCG}12O*Q$4fl|1kLt{MTh&i?$jI?=CB~HS%zRh!E%%PhpVywKO=z2CGNwiff7sQf$@SRO$wmMC&_T z+)+03sbT3Lo{Y2WOZ%ijXmwC1?jIxx(4jUWaD2<#527!xCbeAq-!Qq8%8OX@yvp&B zUP>usf2RO}LkZOQ49kAgKQbBp})T|i_DU%lir=3`m@}qvM28KxRBr<*9U+pGp z!e|G_lkj)%#hLN{iq=-98pQ?=+_LRJ@Ek5IAM9qMqi3A*Z^X~4xMth~C;D=d?hP(R zqg5YqUnC~Cte)IK;$msjyrq<~R9CVVxECk~9~olhCQ<2?OsbZgUq)zuHpd+U(|Zq# zJbxjT?x&40zAVMpqGlKMn1kWnuo}>Z5-ODr*=D_>GklM9E}=Tc>5C75!7_;4bl5|7 z0f8jE`%PqW)k0Tu2K(amK}F{S{5CDtzw2A#~YzD=caXhHsIURBCWR%k*@_#d%||JzLbkDoAE;GUpnFssuk(w_JUal1<~1rPMnGFOU!OIBu`*mL-DAOeP;8{n?fQ zIP7q$c~=cmu^5;1os)r6XS3Yy_v&;klTnO4(xqu+Qa4%#YLHqkr=>};eS>2Xt{uRO zsmv^#-Jjf#2aV!0_Oo!Doi9mwFmO}_QJkXj8MEM?JvS)J(U)5Ol6E|`$$c>wN9ne4 zc<71dC66j)cUY#`qu31KME)G~t;KbH9tEA&n1m?)CQ1_ubJ;O&*N z_6_A|eK%ofI%|z15SolSd6k7u-Z%woMVz{Jcw2xeWSM>A)u511BZUCXj3$?p?TQA^JQCl2#l^ zK;;$OfBuwG*UXA*eenF$-w8=PD18>+Sr@KsFSsZzTcGK@AU4eIq3*)bac%uCPFYnS z2jBf#1)gO6w-q8n!*`t&FCvh30F6;o29`MG%p**ATwkgvY;`1Gx_{^^7Js|U`ihEQ zfT9HqVCH=LjMo0M#42d_>jtA+eJ?FT+-Vj+lQVFnec}dx-shc9*WaDje5*augrqr@ zyLGB?E&JH4(F;(!ErtGa*e-H~OP?9lIDh~E000002OPH^Y-GQCTV3fH+!Ph2X4VyXovvb-|KneZ}s99XQ(7k@u9cUtR&d1R9}Z#jHOg;T?;e?_+t}X; zZF_*WJjzl`+2&YIdS5ZUTt>w3aS|BHp||N(8EOlNCvfb3hx3i5D24GlM5fOzDG~r4 zlDE=laK4sij~~1|oqn%?x*!5zd_&@uoo>B$n1B_~xT&xX?_*HRosy*SGpamRwWeK( z&Du16AN9gEOQL7TZc>qoDbB0E&f%v&_J*Mh#sEY>wL=YsYmlFoqv_@s)X}09vrdHe z(_M*}ZXC4{i+)w>wBhL40}4-_C&WUqVGSX~Q%k~~J920HTb0R&gk zcQZwrFgx^|yGXhu)u|;Pk^(hyXkdi<(MpBSd4QyHx%zA{Ogf)OwHyoI~Jry{AN z+2r}RHs#4j_eeN8HvIYl^r^yvdp-9)PjSu5X4Pk4Y`2*fOA#yJ8Hr77vzkT+icRrg zPC}bsW~@*7lqX~9Z}YY)@Cp9XWFI%Rf_L}m#wB0Ms?4o`xWKpr2$2-fHReLVQSm}M zE5QX$M+i^>`Wgu?Q+5N9vLCv*UmMZ*d-W7t@7JE0lU-;f7>KL)C(D-I@YQylxYBv6 z{Ht`J9l&j%9+Rb8|KJ5d3V=hBeY`fOGyDac%T*31`ZJP`|=A zC1o})kNDKSmlE;In*uxpQBf4zlJcZ5q3VDD00001$Y*g`gez2Kj%yGB6PX&*7Mt@I zczUYXVYoK{T`jttL+@m$+8@Hk2@4sy~4t{w|OK@es-m0@60&n(leZPb7Q)Q>JiIbHOnT2rT z&FsLtMhcvV4-IZ=7fySeeg=U@{fxC))k#Nnq@OFoGmj`P_X$Ec#~@@<3KKi1fo>mIm*)9yu|35~Fep}zcqPN1 zaHQfm01s2);Wq{1N#;NRs=|lY(Ykrn_$|<@sc4S^B6ZE`Yp@0hKY%TxSf-`c zeaHPWTW_c0(wW$8daERpl6Du7D%O4AnXgLIXz7=i8G49@N zFgL}`z*E1Wx$pC}C)h6kwt~*Zcop4T+wepZKA{R?={Lh7>cF=Y-VIvn&)bJVjDlHV z?-NKJh|my?3)~Uu+t9OG_rp2o(!OvmM_3(yJs!(4M~1_jNa#`)cdOJtiP95_c!poi zsH3hJZ8Rb(>{*%Kv;yor_LFy?aWdoJAG2rx;_~`{X?KgT_w@fBmrz^Wb{ZU$Q5KIN z=&s_~gW@k(B8>Nn@FYbVsnHGMh~xTLnHrg3;`Y@R>xZxi4RSI#lO9t2vl`?*1T6ly zXzh--!4T``%o<%HfZEdYkzqfaDjOqCAXkM>NTO?igb3Lq8U`wy?ZRAMW3UI-31(9u z!qJhDpt4D1y4yMLf?YQNl%dqu0pe_Aa3Hi5yZ1Ok$0S_H$P)#2AHmt%n4mkGMS3fu2Oz~6{?4dm()R$rkkT#Z}`C$1~vHc z!B70MA^`)4to+7ZLtP}LhHvLUY`;A+TpR=^XZrGDs4$e)7Tq!?6TxsX2F-}~1VSDa z=sFmQVbJPhsG-RPv&GWF>q>ZR=;v4x7c%2Y?bsDrE&U>hxT_><48M~D{uFjxpatsF zyPr%LxWi_jEs&Qp_}(jV>!5K!X{BMjG!uf+p~19jo0@a?n&#a(hVle?gGzffDxxfq ztWCpkaE$H=0i)K?Dm-y~Rki6${8(vN<)q{@afz5brW;W#W&kQHI0awOUe_=eJi2{F zlz$)$ZSdkEF|>T)I~x`Pn!WCz2SOI#{LyO3(=?`WHg>k-KOA!YO2bw5W}J5u!`a;i87C4;Pu5fF9;nXMhsf)(MKk zF0%~t{O^cZh1xqV^`{#5e%?`P719p|5x%PaDrtid-0Oz0f*yIf>#dx7J?JQ^Fr(?W z4ifvo+SlYrb*!n(ju`pmT63EGH+eAbSG*l71R^~-wrxDia&oV;Fb;Zld(B(~36BZ9 z7@DfFzlVbOIgL9?9ywH6`B#o1o_1r$^KHGHr#%7#zeoO@$_S9V4I+y#(m4Ct4Bic? z(gxUjP|wv`%F8Ae%|>01iL6O(E-(8imTByS=}W3myK5KODWB2K?=W~`Q!}SYEh92t zuIe~G`!CimxZ$ArKg>xHKQP(HI)I0=*q(R$Si-Pi%OI7gZP}E5WuNuxHXcB^w!iFv zpE7U@(N@o@@uO{oDx9m^_E3JVsRdymFNcwOI)~Z2z zXVRKN6>#e>2~B=*85I@YCUYtPVCp839M_gi2F*7-Wo~Q*qN*`>=wigKryv9~Vm+{2 z9?RnH;G9dwQ!ykFKnF+m&VCAhnQT|P_9np??u+2WU8uXv$V+VCv_!p04wQxfW3uA# zkM0U2ib9TIP2WsbrQ7m(i(YoU8ludExac#U?Ujdshkw`lXDF=;$ctw)Cvrr@ zuHO}?GX?;blXE|8m-qWxkj}$d`#8OD%K!iX03f8^KO++$ zgL!1)umg0~0FcgF##d-rrfiA7Q*nMn5lH6%ir{IF*V4R|Gf4u2uuEA5azyv#SLTrT zY!c$^EldWKlyYgFkIQ|w&Yp;*`pGSzs6Og(Uhzrehz^QT(23W4l7T;=o}}#G1W4st zSdaOSZ=8O7XBd`g@ud-+#53d⪙M~EQqwm!}(>P-U@1Ptp6@{4Tt}D@`qh8&tWP& zv~0USUfyC4SuGR-r3^cGXY#gH-gGrKFidzYwWa3re!R1X&Lczj7#sAAI%C||P{%gU zpq?@i1h!Er_Du(Qq;yLHkqLlVhusnv{f4dqJhe@5p&OHxJjor+2CTVtXGOEbzhMqD zk)bJz?nF---KIb^9Pz=6)EP{`^Lo*r)hwAHHz%%0K|Ur01ffGG;00QJbu!oKId?rp zXCpIY6e${1Fg$i=XOIkxQ;>Vr?cF1w@Xf4o7~_(Nr0->+vPK;IoSc~Dx;}y!xq+Z_oDLBff#?S6kz}z zNETl*U6G=!K$oQCRnCHmdd*ySdkP_kkDTq5FfXB%8vZOAic6{Y1Um4r>g$$M%G`CL@FLQHQn~$u+I0lA+UAop%Qj<* z>E>wV2TeW!*KLOkanjad$Zgf5o_~J*Go*7*mjc84TUFqeU`#Y-Z;_S)4*)f{KZ84p zT0L|L`Cvh4n^a}CdBGD&?94rf2+iOxvXj4}U5T5T5}N}#w9+V29(89Aeb+0}h=(lA z0>7dglrM-^slVig60+h51`b_y9Mte1Tm*?-;mNnkg!5?d7-Q z&>bXGXIV*846chLf2M8y=-kYqnrn#11x9Z+2ckkI8cYKsM4lNC6ZDmJe&Y+y|9`za zrhtjeS5Wvq%f(tz$umgij}sNo`L0CQ!m*bn7^%~XhV%<-cT}Q-Z9HiP!X1ZUe5AXY z>BIagoirA?adTu)?zeeid3M?Zn>SIaSdQa`OrzuuXrpre0ya^(HB1Pq*m;TlFLb%V3CZDWa_v|e z_~*VM1lSW=gD;|#?uFjuhr`~gAtAtMS`PFuYla~v(Gs05WZAFo+Pp5j2nvFQVB-hT z)M>&~ZXq3$LSObSy}r(`2p;#Y{T9MpCYClIn%@#o$HnK!J3LLCo}%B|60i0jlz-S;OmBG}Ds-0V z*+Z33so+zMq_#7;<>xX@|4V9}B0(;cscFmpV|4567x}{g!XY!YY*b8EJ30FXB2Hevq5CR}0ith= zY~`e`al>}(Z$}SLmuZ(dM*6gq_%%XmX{vZRbT^Ep5o^zEbCZ%NMI#g}2vOo{tWNu) zauNXwXvA@Tcwg;~=$}e~`V>Fd9%sP~sn-uX-z29%N4O7JGl!w= zhu2URc@ka#9;plOUxM)^4{!hg0004%)o}2sL`F?qjp^|xpG`_6|3Soc_CEa{ifJGp z1z&@+hubf={qN-DI24{Tw^R32Vz{qat-1cXW@UIi$KNAq+|&(S$A-N?$IkbJNk zxC2MuBUU1xu7f{*e)rs~mw+;doqXMeh}N^6GrMXTA8B4lGTz5nCSl&)4r)zU!Si&ez3tGTzDm*%=ln-fLv`C%&)a!I3dNQoyy-Z zJ9|L^_P)YxiXais{Bx_w$xVmeH^dR$U+Sjq1rVff!w5PSd?K?~G?E3ZFQR;)6Z#2b z+r(1V=H~4iC5=WYuS^CDUr8Wcy3l1jdN=#zL}VJPw__?2ag3UPPw$gGj)v?pAv%2& zZ3LrF_|kf=kXFJ()Qb1{Lw{~{VS9O<7d+DKihwcFHjg0OEz>_MTXW&+wk^~d+XFzw z)uBWN-X+8!1Vn+*MsR@EToC#90NurYe~9(zRDMGS!AP3*-LG&qMX`7fh>C0juEPRA z%E0LG`u(uFHhd1lyZxga zc*FC>AUn36wQ%eP;q(M~F$Z*CtMV)k4Y-oP?~E{|*9#BS282$WJ4R=66Ru1HAS{L5l&XI z42!h$1{jIuW&zx72jMbve2{A4ZCKcFl4U`KH<)ORzVY8 zXsm18)X#qShLbTRrhX8+v;HR8X>ErA&W~g247YrT8*1)%#ID8IOwW_>nCPGzk}HA1n|eWY1Djn+p&h+bk`rX3AU;macZ;pX_>p9%gH@*MHZZs6Luqq z7VBr_XZ5L!FHsnWeFy-`blBTM-PU3LHek2V&jS<7H973x{yF5tAJ%aw>XvZ{-+Zjx z<2fWKXv_~7RtdVd)cE~-(i-f9gXwrsfHST;0oh@s>2 zCx`lBQZO$==lTMwAfs`Zk%g~~BJPDYGVA!T=ooHy6KgbIfgkHCP)mwG(Fj5y7qRka zZw)hLXo~3U|1E?yBXkJIux!|H2bJCyzLw#H zqoN*dD_Y3jU%VaWwRS{*6|nSyK?d&q3i2+p+26M~xf=DP=c~Smb#HJ_M9(psuAq~Q z%@np7pb_}r?g&hRo49xY@(@aKA76tjsuj1CiDDZ6#%M8{FE80tED&wibo&i#!nMbJ zC*?lKWAo?WR&vl|8MTrFzh;S?F+U^S>@-#YJRu zA8kilp?obV+67;_JW??}1O}Cf1cT6Y_p08RMg|yoe+-CpbzYh5bmlD8Xt{yU&qORI z=e3t}56o(GQe&q@-!jaRwR-P=P#zB>fp@Sl-VY7ZI&lq+2Hz()j`^KM%c!fjA9(n5 z#QTtxxh2fG0d+kCVmE=FmazNPhqO3Z)2cFUFmqRIE<#MJkS@BO2+n!7Wu1W@$SgoB zOejE6FRKurgI*)gwJ3N5LD>KR11=&LboIh|%}8zBZqTX}DrXlY&Gm{j7fZ0$qR1j{ zK1ujjtsd>=7vf)76osi{i1A>(MBUys()y8~!W$OJ-+PeR<}w((DsGn3#LyM_AP62+ z=Sio@`vDWml#WkKI{ON?dyle3HRGr#AncV_{Nv5d2HqTF@~B0EnH-#)BH(dZpt2d2 zkx8I5n&TzjdjAM$DLj`UyC8$qrG#nS-CSI;s>Vz?Az>Nym~voYwik^P-_TDQlC{cy z75UY0T}*Gj9g*=ye^~c22s{yQbWn?8lWq9bXey%N~g)lmAfFk~7?s+%uyTyRH zpbOuIK3@YrcOEbR00000(fDF%{VE50QEN!(LbJ!25K&-S$!?91Kt_7YbK^IYcZwqw~wgR-WuMCY|sik4S9x8(i*&X(6 z#nVraNnVVe_})o7`On+$d^%*xKL()NUQIY zME6fLM@Um|dy`=_3gP)n;Qf+bFrSZ<jl|vdbjngT}KI1ob^? z&jS={vQSN%YIebz%rPFS&U(8@I&Q^R!pY7mw_0&~ z)hIo7if+SP&<;uZpgLd-UXm$|0}+Nj3l9Ugktys4l}<`42#kS*!DZE!D08gyn=W8( zUm9gxQhxYD95Wo0ii&fsxe_#AW?WY@rCK6UISDDfnQ1UI@TNU&wuq%CcIDRto&ur! zNoVfjh7p#@J#4*mCz`z^t$nz>a_t?dUkgO<{eYN@mH@6$gSWgub(fNJ$KtaZZ&1Rz z!wO%_hjTrS{ib`S*Ieis9{)gZqti?7tQ_ReDk5Tv7wqHT#Q}1N=sK&Q34)-=A*-tr$aLT_kAJl2>7K)iphv z{#m*C^n5ne@%`?xHc~MR!pM8^bst>lh~n!WaY@?_O_$eRQgn1jf-z8J7qgrjc9{fF zRIm)Ej^Hxgj-5bz5!~9^J~A}oaao}YtqsO8ellCAW7Zx@ls}&Eox4S^3b~ee{%b>| zAMr50$7k>s56iTD-Ag<-S&6GD-RgzGFx)8#EDtcMi&9ud!R6uQDw( zE|17Uu4|4VfUbwJX;{=uGbzW5pcoSjO2=cMbK_hevqrCQdS-3HfW0pEAAx3hz@;K;pEPF-m68o7(OWoEN?qNkh{oe!Rqz4<85}u@E%)Ub&b0DU5Ch z*c;0Jmn8Gg*i>U8n`}YxP)}o?p^^@!QROvvfZF2kn#9ea<{-v66>t0R-a=6vlH+2n@7pz!D%iMMr^@J^&S zbUv#t9EtyG*1!sYAP@)y0s#3l5n_|>IC2nB3DRW>;kbj~f5jC+YM5i%0~J1DFWbD9 zovxYG=OetSD$Zw(^DspVji9zPZIG6j6&Bckj#`*ISho9g$d*$M2NSI zh^};+A|aaZRO?QScn$9x`}Z|bQn_^rXCY@uiFS&THs^i;#dzDxE^2D;UZzCK=+-~{ zG?eJ>n^S$mr5Hzfm$s7Jz0I?Az`)k%wjQ@-4B01-01oDM2LlXC{BzVaaJt}@?4`Rx z#QyA#UcYHAN=Q1?mUkV)Zf#(CI+!twzW#(4U(h7U9Za@7lR2ei7$xSAc7L&bj1CeD zXP%HZ9Gf_E@&_UC@SFjMxXD-O06$suHMVY1EJZ|=0KP?cxlOzHR64e$4Na{#KI3q3 z##q|s8kY+t*@fSMqD{2dr^Q1A2ACJJc8q~lB*rb)+=1+aunuCA-#eeFIwCzGk2b7W zg-zK*Ov3~+;Ldw4NM!{EIZqq)C+!d9E2affOZGS{Y-pKom!g^&%fwusM~O}t1PK-i zBWJFIr2jst$5Vz!ZA-<_ORb4CnX($0dNC`1#u%V(!Bj`8OWy1TPGh`WX;KKh9nDS7 zTu(%#rR)lBFO~yE$vV~nhPZ_JW!I-Dwj+H20^|aYg$kO;JzvU0T6rq^MJw3~7J(FS zd+@KYCHi7-n-0MG^alWdJb`ueHQgG&6;JAkbKsO~a8NgqNk+`-Jc*2)FdtWim4}Hm`9LT%B=9 z#afA9C~N1Ml3brlM9FWsQ|!bhl!9c2@zaa{>>AuOlBI94vWG> z!s(=R=I;8rr-ySI+7IC44NyQVy<}Ikx-k7$m-EBQhWU3F>9HPTB3hz-GQL_p_Sy;n zvxO>8#Gn9L6UpUN2av6G&#n%%g`B&z3Derp;YK+`br1t8BrC7xl2seFJXK!aHkNF! z40r}1HGcmymz~wVNv$tQ{W`VoZ6bq(J*$>AK`Ds zs^1C67UJ}jcCSbRF44g_qsqi4{vRle@A(gk@Af7-iK2saygvNJcYE#0{71T>Hs%Iz zohKt;L|}e#&KFK|7ziSWeTxdAg0ydDWhPujq^S;@W`u5GGEc=Qazp1G>*$+Lrqs=$ z=6SDg}K9Q+u`ZqjGg^XgJ#n*K|_a7Yu2Z z8BJDrpagOVhdYG&5J3yZM(!rb9l%q<_bGN<;bvfr>8r=%- zHdyQiQHWY~#~>-KBxE&GgPPMS9`A_nNnuaXTpr7kVmFk1Wbd@cSV6ny6!E`?oc#f& ze4sEJ(Vz2Jo+UaRplC%BqT$kcj+fBq2sOHUcTUztm7cVNhh`*BT$ zh2R!I0-|hPk}WQ%!B>hLB9zp0gY><34mLx`C*_Q#EwK znl|zvf8B3}EjES*p+Y=XX!Jq2prPlxJtd2vvFC!f`^?RuS^Ay0J@cE$co3C|A}(5E z5Rowc-(6N$F*~8IMU_Z8a6x+`)4+g4Hs%g+60GHth`)fn*{K3J)x0I2vLmlo$y}RG z4V$>H!*E)<9$4FaSC(A_WtMceKBQm*UeoI_H-kaYzCk|_rFaVHYhY{YM}6d+`}3Qx z&#_7ju%1k`lcAZ)>*@W9vz}cHXt`39ICriJ1#jUUbm={CPiacC&>(Ei+_eeQ|FHrB zRXvZ^OGMBBPL)&rT47W+SjgiRqMuoE93M5*l!QygOUy`V2Tu>~DSl8shB#&AGd&NV zCv5$3A9+EgnJu<7R2K}EXgvBYbsbEBX{R-aQn!M-`eFC$xX462y%%(3tc0LiE^D>Avv^TL~`H_W59k=E$ z(_EzwJO3d=P}RhchzPzzo|u|MrZB*Z?-o5BLNI@AJO8*W4}(}(#sLsIK2L76a!Y;R zETzX<({zY(CqDr<1>#BO$Cv|W7K()S3nhze5=;69cLv2@>_)kXw*z72y)^4#Sf?Xh z`bqKOH<2Z==uN?@EG5CflV%0DwcIy`f|@dvsdvo2mdA4xeS|ulv1UwUi>Go>ko2k0 z(CX|KrG@>hXs$s)sk6v^iLX={;gEOsl?0EL8-5k*Tl4heka0rmo@FYO!}vFm-D15~ z_v~@Dq7i$_nbF1u1NQznyc2FtLU0ximoen$!@c`W0`+W2vFFtcJVpyXyf<#ee<}*_ z0esRrvd&d44D`7iYB z{}(dM+!{Q1-H+2Bv_nEbI97wO2oK)(#c|Kz<*QT&oaU? zmQ5w;!ze+dp+5iu62V$#dP;FB1u|ZIed1hyzdsm9SVK6f%Q`2db5DHC$exe>d5<=m zAZy+A*}X|ovk4`P7=d*0ApL$`to+?L>%2p_B51I{tanDi7|f~!{a=#JL3@z=5d8zK z0BddaY1`{ilMv3=uM1N3i|6l4l`relaP3bzYIpKTkEA?e*i@z=GAnlA2qTT>h^#cO zo7v|LdL5VXFf94DD4gpvou12VO(EOap;T&tZsJT2<4BoW$6jKx5GQhBb2=@?ga5! zCV5(fH@zQ0y4uYU7Ar9|>p37_X!+VHJu|rM_mIDXj1H;5{dGbv2>D$_GKx$9o3)iS zw+kMv?GhtVm{YUmOxMqOnt&*-I}&S_c;`+MgHO3JTKQ{00~Y%DLBH=tvG@(T=cD8z znSiCnx|$QUH8I6!3JO8ar4$D_6{9`9zN`YA%D5!C#}3lGrrx2wg!urRrUEHUrB^qR zbHgUV`C2wL+rzqRujf^g>fzaM*Tm<%I-v9&DCgjP#mA+(Mw*t~A%|E{XHF>iEpS8i z+KQYtc*&<-{1_HW>6$O)3A8vLA-#%APv^$X0X9PTOrDV0hwLbJMmZpDUfkoopk*D> z;Oy~I%D#fjB*!O&X%=Xuxn;$kacYy!{n!2uk)t3BXcGS_AK0Ms%vqQ6XYo!L{5WRD zXbhKi7=Xk)O}I16bA<(JB%&z89>IR1jttUDu1S<0u!^U*B?ch&62isemn5(5W-wJ$ zgx&-NcOvZ%&$&;1WoDGJtqY{%M8!*abo{_?!L)7agdt!p-cvl?fI}%-$tJ%bG@pFk zGgaS7QDl_ze0p*=v6`k)Qr`oKGs}T(#6`3X`X9M!X{&lK=sO>#L&0)GLRrG~EXj9E zB>{Y&EaTvK*(dsl7U8dvO|z9E(w}k^pcyQ{GnY1uw@>WMOeIpZr3c7{hZ+X%$(m%X z%W>$st1j(n9c=&p&jHYNazkQa)v|8w6!XQgdvVdivH8w35}K9+PU}fR$)_wx-%lA- zjB_^#Lo+~$R;^;(OQ`toty7~DMs?cx-;XjCsRZU~cSiAKNp?En_T>*I9(8+nP{XW4 zz%}jS@=!P)V0TOB{<(-RrOROfXev(;j4?&fC@!23L`hGim)ZiAHqHm2Tn&1dZj@8ExmygYEY=ZhBBw$>rn;ajuQr z4Nqi8P9x6@2-9ZY%qpJF3$?sW;&QfrKBcgmlQ&Ox;(H@)I#sNtt7JU zm;#13U)`E%N)Sjo2QL+V}Dx!Cdk~fr?S;Di@S$&Ne$j-<_h?q z8`f9pv2XXhXjY^k0ZUtuSvQk1l-ml|9$$WUq`ZWM@qq7(g9{}Br!=y^V_ATpYA4SRqai{D1(opywGkrEABYF3r54N&p`0#<;(;(rkrI zhl%W8P?@>Tv{9r0Mqc_2{IivLAPy|4{7`+QhI+nk=D!4F$YbYa4 zlJ1PGtC@-OXms33xAk$wXXWxeKB;=$r8j0b4VU1xz}KrD%S&>9j(1C2Y9{m{W3RDL zE7dt{s6Kfe60cvr(Z|h@ZC%;cBnZJQf2xC|JOI^Ne!Y<^ z8b-MIN7FO9+8^!-s`XJmQeS!v0#5yM&qhs@#4r^tn zJDRu2qeY|JC>5=q3;w{HM4-qM(r@9)*`E<3tpzWD6LRi;02gU{FsgkOR4+Vp_Omh) zs#frV{pO1^Ox<?D$uLu_~#fyL&rI%%w&it9)sDITaMV!PdU z5msk(b>`G?{lig5khpe_<$T)jg0+y1P~sfH2yqfMN!(TYKkzJ zRX$nt79@tf{}_P$Cu<2eWhu!R8sM4-;thuAFr{NfkEO!Z-Xau%)L(+XSdzB;+M)rZ z6*t6Ix@&0N{vh z0_!Bf2In>~_&{Eu-@C$uq2ze*&u75*rU?MX5V9e`f|YoWb8nHC?R>M!{L8OWh%v}J;bh%ctM z<6?WeG$M}B?FzHYcu^}a&K`d{ZaWn~X4pC7i{Q+VF%OSjj>)vXYt z{hSoRWG&EDu{qKIt0LPbJkQ3pHU(d9`fbgk-o4%T;mcgar7X=2iVC$}T`Sl@yj^$u z7U|b!M&>HCc^%yTi-~wkbIB6?o0Sysa(H;QsR4TE@eJVH1}`g>sVHaZ;eG5ta0#Ln z%O&BZReE%Ooj2AP_FwWXO?8R+fL1nBO~mZ%tV6@{W$hH0KSwbPra1Kex#%J-7kiL_Su@GD^;!952w# z?nmm-1lu~QKr05?s<2bORa?EXAQ?Z$eAFkw9K)cv9^yLMX1&vuZu+|p-5RI&r?zt3 zaFgn^_1C$1W^Vd5)GQyv-v~!PYN1Lrc^nOF1Nr@!Ak@cMO_t$@Xyx8g#NvCmM(WaK zeqN)KAzIr;I)!LukhrB5I`(@s1vBSGiK~cq705SfFl)GA8kp$Mvs#llA=M^GSLABF zH^lx%bg5YC5cel0YeDMdvv3gmPZ*l-u>U{n~Lpi=Px;UyF zB{>PO^V<=F_P9}aws9?;T2L%_hMcDTfwU}a#g#LWtXbV=VjC-d&d=r1b;F)N`6~E2 zRh&&p1OGyKjlMa6DjqHcz3(s7&Qyp_fZ*Fh>k+#v>2{(aGY-1pa*WUhX7o%=HNDG} zVwcm%E?3dT?za>%!{Okhw7Y#7NahDwnU=uBsd_H&0dAa&Ui}Jys%3QksmfEFuM)*s z)K^>!0glaF;fLBd$D)O=)aml|t`Egi6DVOAAkdYAYS}&+gZieLHitd7|FbzO4QBU- zArx$+@1Ne<5Qy@Yw(VpvW<0ac2L-J8nf!Fj3(Px!A{Q{9I$IiA!85qlRTskEy}OgK zt#DMKmUcf7-V|t!TH_;ozUZN@SOAkvm*FSh*Gx(8M1oCVu5xGn!FR`)8cvJ@Pg;UP zB~ro<2HwS-f`CZ-?j?Q1t-RKen$GiO_E@?^t^mCRyyp#9?bD*M1Bi7>Ci#wk&le5% zq62A=i3WuNeC63y+7($zsj*QUr!AMXAJn>U>j)+NV+#YUkv(+@%DW>5suU!}`^;%E zm9{_dVk=co(wns8#_cQg&GC@@H%sJejX13#n=A@>*ff`AhUGSZ$qR2j8;#rNWl!;C zCY9K;pr}xD!J`^DGrVtm7t390qxWx)EMqae@=XBD=XgD6Ztjj$8jwdWHzp5ev;7gF z)3wHpPYbcmR^uZcyC7^rq!WMoPWHV_XigxDV>KKu#hC>F^4t|}qeUa~uR&uBA!-ei zWM!<1bd!&2)6d*+lGqPW{2RAlwdaR2fNgj{KMfvXsP;ISMh+w@FsGyFF)$e(mutt6 zERJXMPYU1jS$)(Gl!gzQpeOC7z{c~rNowNl1Tv0b7~ZmH-;AIcb3?xzA_rceq5G!AvtS3fE2XIyb}%ABaCVRb z5>goj=z4%7p@+?Kv2e9MY|kO$kwrVCQDLth5u zW>gEc>)w1uz04wuI7K*De;{g@kB)h~{lnciIQ?Oh`w6Knv>)^^64c)$AbvyWy~947 z88D#&oi`ctkiZ9_jrmYhTDu;e1xEOM4t^cHAE0Ixc%Y?%rZ%!)mwHG>YULKiArvH% zvpy+FC1!k5l1j|@r6iS^@k&jjjaI>|e>_gKm|16?jHR*GOrb@E5!FCuZJe>TKj~Aw z*0ba=leu76`aY^yy47Mg0seG{zaz0aJRx0kdeynD{sT=()yaj~P)QSdPI&K>Nl@+y zIC5FrCp+=4A}N*~8|GT{AP%{wnE>IQ6kxghNR|%OrEPpuZN`2JFvkT+rzqg}ZgrmKdP1Oy#cB-0bu6FTWMF*zuZX!vKuVJrC-)m+9bfyz|7)S@)B? ztD?L7a$ZJveX{s;ryt~a%+V(N3-zyHtaZYEH;GyyB-_;F{{>)%f0}ZpiY7!XE~xZZ z{+feA4P_%)2%2HtH@y;i*03XD-Hiruj$X)gwsChT(_y?!h&%QH4@uX2;HuU~I+oN~ z?p2H5&?fd`S+b>T)chN4sZ2Buh7@U~bcEYheVYnRJ5wNH3l>(vCs~$QU-F2JFe39l zE;aPx$q;3)=W&US368^5Ci5MAfGXJ(re6?(w{O&`(9BPgU>j$)Ppz&!kp{`0#hEDA zwJ+CCKvF25BOz3ROc42oU`e6{_8Ojf-VH1&sBC^oJZYBtWw&<}od0>EOcjJx96R*5 zmdC|cp0%WRi=ndbl;{=NwE(h6+lv|Ab&XIN^Hu>G#a;jYWK`H0j9<{7fkk|0SEA4G zT2YZTDU|}1I-5C@0sJ)}5|@6p&?DGX*MxU&XX#aDSDl}M007hyLx>|!i-m`jRutK` ze?Ckjl4Q*hSz`oEEae(ts8qkRC2R9dUbU{$Em5Kk(kMsBE%GejGb{2%3*h!-g)El#+Q-fQ|zQ6UP z7YBUPm9h5I0*9!8y#&R;cyZAo_f`9~a%f)ip*A6sF9d+NlXS&raKkSwkbG4wTNIbg zb<+>gm57Lx_%5r-V5;`ddsO5<2E2zp=Lm*-MN%MedpDh#42%spR_8?;!DFiJra+ zq0Q04SrI`@YNi8yD54*Z$9V0Jlco-ITBcb zkV$|?JI-bwtP1KOakeKI3j9a>khwi;|F$APltqY-0)Dx_++S$6adW9d&pa26f`5qW zy3FbV>)1xg!!_kpEKiu_W3En()Ky$=hQjaVmcfc8tc>D*TGw0lla$B50(MwBS7=p` zXV=MQbJbntqpPO@MlLr)R)AznRU;ny-ic8^6#JE-JZW+TDNDw+cS!*2BGs3uiM)E$ zRz;*h#Jc|X!}u&V$;64f?ul;?xz#tyQ}fQ6Lg@xQNmtoD+|99}q&cKm*(D`$bk3RD>=eF+)xjI6hXv_H zTKpolt9GHh++Bz~L*krQ2Bk#FRN7c8Ba2={H@-Col3YY6WQp;u2`O)Gigt+ekzvn4 z{4%pxsTwk9qwvG}Q*D!W*5))cF_-Q02QdoO?=>_cvk|10MIbL=BG^8z%VNI&BQy!q z#n_G@C0r$xC~M^aaWnt8=i*AFp%wE{BJuT(3cm=ty7 z+;LXje-()4w6l%6sT>G>qrR?&u<657%ypQ@(2cyU;bL{Yc6Y_HW<_Lq0rEYe-=xL12W9hE)JW!kIupuuM7Z_8KeU#L3Ts!o?dYgIFjSTQy zA{zqt*yXV8n%^*K4SeBe<*r#&8-DXMWwSbfh~#vZ6f8UV2=ocVX7S%M##PrFtRQ>vo21qNDo;vm%op_x^80!IB2V?jrC% zJn5BK^wi8|)c>B&YkYIc^U&k|jQNF0(!{=f2^m+Oj(1f!QKa^TYno$mY!Gl+w6b?z z;)WQp-2*gShP+$ccGQr6)LBxrC087)E6mIIJj86^Ww;)e-L@K}S?IasgmE&d55{@> z$E+EWU$PGc$Ew4Q^2q*XeM1kE4a}_Mg0nfmUe!S2Iv%zyy8Z)_q`F&Ytgx-v8PW&U8fd6^e^ss5MVxwK zQ+wkN*Eqrn#ir%8zQukc-og2CH^>J(^-LM#9H;$85`)9K&NkO#8cN4G1zRN*3sN9I~7O%Df9gv(y4D&QM z7(1(NXWFan%}7qU+(l5r>fR%=A0}6;fn{T`oOd;L44d6*YmCmj+?S{he|ty4V%HC< z+Xaa!?g1E~*8DGX8E!zBFpP-+HTs|g=ljuRkEe+ex(6iYYi~6aN_rj)C@dXIBu1%! z_n74R#^Q~xH0Jh!AVGrMtl@1!PYieG--{HL&tsP z%9{bC@~3Thy&3A>BqyrC7(Y$hGXphgTe!JnY74R3nYSCSnkWN;TfSwPtdyN+hvP|9 z2FEQ_yG8)Q&Sor4U2?8nV{S~-90~Hs79irwZrg#!ENF(?huMK^w;Fk(k*dFH$?;)j zp~#Uif15MLB14H4wH2Y|FMwc>aJ%`wx~ZZZHf;BFHVB21XY6p=VyuQew(?02B||~g z;%~lO+8m(7v`x+V_Bo`Ekpf<*Njgp8k^APC?2LLNR>;QA;+vX$_1u|r$>=zJFnXpZ zOeW^pj4?MR3?$xz^F2QrW9Fw3F|wZMIw@!KHioCT@zTv2+8fVIEfKC+qS63Z(bkYS2~2DG~&i+-PPp%wIPxoeZfVp zrdxbNbH!*{TUN3_6YCyN!Z>jBBa?A>BI&YjcRZvGZAn)tbFmYO{jd3!8s@QyA~tWr zwwlX$9*KOF{$Tq~AmTm8f(EH_%9rV4PiD4esP|Bxj=Tm-IxXC<+@u~}5XXN+UWf}K z)YXJ5-lvVQ#(({6MP4om**JiUE6|k{S1r02R2yU635@CP3yAe6Nz`^Rr?DGCa1Elb z+9DL?xs{?M>RP{PN}R=p;SuVROL!+d#{`Ig`2Ym)Hn)1qq#24_oy3)zYHjlf!nJqr zRQ6sgB><}1zu7h-o-Q-?ImdI1MRu)&^d?C-{+jinNE9JUA%mnhw}#xl09@R6FH~;M zqspvW`ebJKK+XjF{FJc>m_0nNkqj2kEH_F8a}(Gjcd{C^GL8Czhhc^ydt?f6+d92& z#2XE*X$yV3cy<`iBWyNieYQjf9AV9w#Gvz}uuSFF9%>6)6x@51$OwBZUoR+`%fscP zEfVcIWRbtb#k)QE_bIcnAPd<50#1LQN7ZG>*@^iO%Chq#eJ#MxWN*YFUguXws+3_~57~I*2KSUY->(xPCK>Xcor4-*TGcbteDN!nyr}n0^ZwFybBN9{3vX zENHHS=Y796#iFA$h;~!8NvkVBp4;rpMvjQ9aq7}V-lyzCvfY;7hym4=Kq&Mtr&3&b z?tPvJ_a~=-!Rh z99;YZ#pSYaR_HwpR=dhoxIbmB;xzLg%qJ_h)#;00V?I%KpOnC7*8d==CYAnmUlScj z3WY5yM63T98FRVSr?8vQ(OV~?Tu>}ACro4QGHsKM^&1c~vx6uS{+yb@B-N z{R@_F)K2f8Nx7&YA)$?GKdtUDdI*M}HZI&w!Nchs3{U|0m^~LSb#mcRw}I=#rVO z7SP-GUw?o_h?IwSzvh0ax;I#Kqv?=D(8NJaSRF~40@b{{i1|>t_9Y8`wunjU52O`G zH*TAQR%jBNJTARpI`?pQuApqlE<&|h{)4wUns`R*|33YTlj&?lv<-}WA~0^MS~jx5 z5-e>LuLnfHJ}dT%i;W8Wrg92R^Iyc)4T6L>&X^yp)uN^m9{Q#>^!c-x^Q7^1N>zn zvl5`**{aRNE8st!?Qz013xfh3lOoGocX-b;aPh5mEyNK*ao{gItHUnv*X4_=x|O#+T6E|5KvK+Ey=>6ET4d1ODgFy# z=F40OPYYh_5)k3G;8X-$WZ_h4SE*tuVb1fgO-mN&c{0UhBhRw5n zpHGS*fK$23aevhf)V}GyV2+m|ul{Q~(|z+ml%OP=G4#(;m$2W51^`=n$kD)P#Dv@m zjVR~wj=6p)J1)@h>Qc)C2_-=-Bs1C(4Qg;R`_vN{s+q~j*6Pt+kY*xiE}veM0002G z@}&IABJrgo;$OSXzC4?p{PeKu!}y8^*aTH(i!PTD zTqQU4q$imJ$vkcDN5U{h`1pxqfw3v++;8q#VW>~&*HQ!~Cky;Q=yN|ri%EfmcC@`i#%m4tBU&@5t zp3V;2=(~cQD$#Xf!g=Ba{bDl6Hn)*IM&$*!I_asVU1otH*?t0$IoHp3Wkao<>@-VM zOM5u>tHp>vfuR>s7dTkV9E_uL*aw+j0`0i$Cg0z_%N2!^IFwVMpZ8sty>F^Jk45>6 zz1d<`kK3f`NT1IheX@xgV+;edk@sQfFP@t&y^aZNs4Uk5#q~oB-$iNjBbwYe<05c~~Q~;MvY<+Gp#ga|2UB zHV(y_QV(V6!Tt1%PZDqm*qM~#O>1}&xG=>6(_C5wrns~VO>t-zn&QwcHN`~s$MrtF;U&f=7gwG0k$iHp4L!1JQMlXlS1QQ%R{mkvja_bU&IPmxr6s}Q4Sh#kC&t=YA$|^6I5EtZn4l4;A-rHDVk&fS^GVf72HSge z=taE^pvxbGcp@e@y*@kJIabmIP;he{L=I$koSo}=py+*)PNWcUZDWp_TQ{fn5?Qd? zB)|2uRO!@6Q*#VwV&cT8H&k?cJTxO)&^d_t*8XpLM@5D&(65=?#2jJ=&!8n%jODAf zvHv3g=lbC+5r0F!kJ7CY2OkumU=TItoKEs%ARJ(5D5488l3dN=uAq$n2ph0;=TRW2 zk8ODKhHy_tee|ZhY&!r^q=~l5PtTWN>M=9LwF z{nMqSVL)Uv=qX8-tI>@PA97i&uT3i(x;q@{9|$i3v!?a4>~sZ|!0EI_MCPd63g(dT z%Vbz=-|k%O|5JVaW;fJgpjtc)fWSe6^9E$>zbqKz_7!32`bgBgzv;(aF zi-r5+=%gY$!=EBD3tLK|Acz2xUG9S*<&e%3oYfCWnOa&`W_P9^?nar;NjiD65uLVT z&+{qWDPCHG9J6LbY8WDbb9O84kE5zoMDeX9t&m#sB6>|G0%+Mz9A-6l#rDp8?5_-WT-Fmu}fQLo~=Ynr{UDiGMtHDvP|Nzg5y zcf-6*#PAdaLO=Nl>pnysHe+{m?VdT0TUi-wmSlwl2R|Asvhmdzp4oH+g|vmunfXEaPl;18AKvUG2=`z12ru+Kr32Y(mgc8Mf9bf{{BRNj z|5}uA2{1dQj0c;Uiv%UCv@3((>T`bqE!9sP1Q2%iD^s3eASa}I;w`ajvn!U=bPqrc zAhJXtXffu_5DS)K^)M3GmL%JmXFtMioNu`QIkf+DR`+%o5(Zc1=)#`^*SemQReRtK z+6l#odRtl)T?iD1zo)%F{P2Dx$BcWJKT@+OBd|b*aBn9I*$`?vw~n$L7*U3~M`{Jm zm`%-?n7lE<~Zu+G%}HS{v*? zU&-fFA~K({X|qID13!q*=1Yc4h}(f`!hYdDXBbK^XNmoDv_YU-UJjV>CR$15P#;dR z!{WzZ)l7PA8<(W(G&7FJvk0J2!WTw&NTx1@mQb!(ocdu!R4Z&AdFDzfEw3RqpM)G@ zw2&kHzYsX%)U0GJU$jIUFY^S_!$B3zk*4784b5B|D(ZOZ#Hr>kGx0Y$d&hg z7ozWY)$m4;K37W-bEZY_c*<3+6b3TWi?vXG04^7^OpFJhShW|vi4_{~Z9GwfBL92i zoA)i(IS*QHft`F+QA>D1UnzJmX7*07ElbjQ@C zn5VL2{+@4ad%-SC``~0Jyir#=n$#!^hig`7<1u3Lvlu@!-6I`HE7}T`_!Iw2ep4xx zj!bT96o=q7ha85$EMK=)QpibaR*%224&Ye?5tT~F63!fX7b{~0j-y2oM!4xQ68sWe z7O#gvpfw>lR3NzB&U075FnDVk1;R;0$m_jkY;7!g5sf?7sGBa4gW zebnB~Yb6fx`5=m2WU%RR#>iy}97vkLjZtI15fdKr2Mwc}fC_GJs>?zK!2quVGFz7S^4q_oDgL%uH-18vZF8%!kddVFOfr2NI ziaC;Mvm%@xK$mBZp^oXcP$BJ_jS`oMW#V$rZy?4GELdEoDo?pEPdk7)gL}NW7{xvT zA2E>cjxCxm&N=kYsW`|7_Dc`9yUpSpsRR?KonRsIP$65S(Vn}(wWWK%%-=^^r%X+< zYna%Pvk$=wwl|gp>~1WTtQ_U^L4YfD{$-EK&AqrxAxN(0(P6-3tN;K206ooc7d`b0 z^l6JwlZiFck;_Vvh$~13%gt!Il?-z#zYtF}PL?y~s{f2hXo#e)S7dvo4*fba?{$Zr zYmDr~oVj>y@uex8&dZ@NArpkM95O|4dNr%&`&#y*bI_wH4cy>HCQG4*Fns96rX7%Y z;W zD1rx0iB@=cF4CJol@3b4{qFc9k|Ess!z17;&y7@*?OF_?OU@{>#{@kB;u)#;^7@3k z;*9GHiM}r%fRmq#`p^K$iLrD{T|8floOn@!B}tw?Y|U?IvNtO{y2fE;JfhKjmEQ5!R<%emc*+uR^}q zluP~oe4_o0WyOnWZiNNt9YpfJK#$5f7*JivZ_OlzpO(GJloT{ZZ)7i9=8#85o6z;A z&Tz9$jXkmmiWb|cp}z35wR&5wv?MU_`5nkmyMI$$W&X$aDZmN z*%`V;&%?78qRSu(RJ4f4cKsUn&bwqFAh;G=SSORgVT<5pr7++xJ%_IPV3}{kYdI@uQMjz(H=e zU|Lq12^29AK(=)-6{F9XovV2^#%nRI!?e#D?kp%hCu)P^Y(E~Xp)<6r?nPWK)(0#( z<>U3(#NtlY*=h-RWWp0yZ>`@h=dwK=2wqeUyltU)VZ%nrlrI@;%;B!CbkpOS-+AJV zh%-EfXIv6;{lTOnLlA1jAL;#tM=#L|7OpI_k!OnUaMdw&_^@a?4I3E0wO^3E!tKRZ zbC<5e^9gSSz|{l(#&uGsoX>>&L*Xb~)PY&!yC7U8$9K2n;qZp3J`X~jpNO#TPr^!o z)|3mL)!0)Pn`Inkwrf+`1uuny|ECSEMh4Afp7ahD?RVytvIOJuZf3&}MecV=aPiA9|c{not_>g1bHGjK^I%4zJ z8jNHWFYXsJn?=D798YiCVK#eH%-(6zO*BB=9jRfv3D}%URPt^}1DIlCMq0=${tM=6)8-HJ6CssNBj4fXwB$3rxdLPePtCW@?+v?05Mi?1EL@O5_CCWchHeImI zl&*|>x3Pul@*af;3|YT%5n!#Nn&`NI8sQWC#b~=BjgNO<2HD3qv~&q=Gaw6fT-D}7 zFznHlbF1{KiPo6x01dUV{RCt7;{7{rGUl8CbwGUv8(s_?8}uQ+UY!$FhP6U|;-$Q6rUEWD|?+?5Mt zBRC?mxRF!fu9Um5O;x$;@;?k8>#8XBbFgi=ylg3}5f~EY1wU=Mls^ks#9%<;Ax>CA zm*bJ}_IygRG2qQwW>pmZg~Qu16lRalRZG`q)v^xjlSzHyp_k<0PjjcDW!`6Juo*{# zt4ct&bFtz3EGwNOOJ`nOX*-rE+3m9v-&IEgJEo@?sOphE&w1(P!KFq$j>9rT4;G_l zWfq6qbM!11Wh1q&vo`w(Bt<*yU@eX>FY_+7W4s8GOgII^!~`voUZ7hM8DLLsgH4Ze zK8mS+;9(vYH9e8+1FWarfR+SxIF!!8Z9KhzhrU3hGAz7%RGg9iKNq+vA(%3upb?>J zyDm#Lm(;ePxk$tIq68Bx%8zzFqeaaKe5O#Co&<;whvR40q%yDzwOjRHz7IG+6`>~Z zru!&nY^11X7{8f|efDDf*z6@WI1kN5D8rf{mEqb+MI*iq)MLk`U#52`^3+?rMqxgT zFwI7$Cl7Rt`vqu+CEHi$xl%vvvg(X{ee>5^G%pNSbpGf%-5laKt89bCl=(hs z*O%-B^HPvKaFHB}V>)|RgZ^Y*d9`!pPlg>goQ!+33?qDEN{t4+(U|DHuO3yGgso>? z`MkF1V}XhM#Ofw*I(^S0?Or3Yk5~yzY|w6;EsbG(p$l^p2E>`0DE$mpy19#@1cod; zK~Jo!tqiU4IiT_3=jkTS&ru%10GAy}xALosH(`fnLE{)-g=sb;v@2=rxM9%HNULR9 z({Hcs&HY?<^KYOV@lVnJg`5kc6>KNC1x@|L9`eXkNGfc>*8sjlqZ7K@4^$8De>a-~ z?u3!oZt$1&PQ?zr6{d84JFxeY^#MELo_l&HXD2 zJY*7uNbxK&=ch2wl{}UEsg0)_#+|Dpdk!YTScdVM{CCAqy8Ae^7U0XQgAn_RM)H>)VxNAtPEG2iLqOB0G$~7K@eW+6bWMK{ zVU-ZIuGL>51Y*LoX4(x8EYaJ8w3t^aFpBh4c5o)dCCzn=k&lz<&;3}X4j`S?Q4Zdz zz+UsueL_xqD*We27LyAGEB%8B4Sfg{+9zpJGMgaShlDVgtJoCrk<94^5Jq0^EW1TX z-IqP(pbt|v6gyj#2|qu#uSIr44iKi|8A$SfRCx{$xOl~2e@8uPmq(+DO{y+X3-eQ~ zh$Cq)9v!#jNPDSaGZ`1V}Q8Ito z69I#JpR-l$gFZne(F3~1TS(w1OBDRz$O z-`&2Aac7JT8rwqXw^CU@m0#BIrv@CEy_EbbEZG*BTxp)0jmvhRY_uQ)G74w(h{|Y( ztqvo0gg4o%Lp_DoQ1hE~m=aud#HA_(*GMyWNm5L=OHtF8k;)k$5j3xvIip_qxc>Hl z{t(BamM1SFg1a7^SoehW@3LaGuk$5bdDCM~M+(}hJQ zkDR63(f;!IM~RH@J$k%<4%F`v!oSuHbJ)9#9}c*bCMG_k{;HHDLWp`O45mf59Z# zxVaIW9O@dka)HAI%t^Yza-H24Ec8z1!#B!5sx(E_C}dQWeyUZc4+e z=ix6H6##{TVFDijI)U{F?C~Fs$qCZB3Dhq&U)T+ z)ksem?QJ_x*4{v%*z!$g)nVN8k#FJskg7a5PHFU2@24hp^9XSs7~uSRnnDll3Svs9$i+6mqC?@zOhRhcec1$O*6TSKlGrL z^|b8>lkdbcsx;IWDj$^;El#eQz={pIO7xmtIO+p=NJVL?^?4Lj4c5edH$CiOGWC*~ z5l_r-3y8#s;ra-ct)zvFuEEXGzmDnZiY~nE6SQ}A;UjIDCVRfpHAA>f{w-&-P#nqw zhQ{^fZVYS-Em#3*_F}vD?x6C)_8GaB{LXzB0=n8_2(_oZ61bT2xDDx6S<)_o;SNi) z)}4sW)YEz==gMl$T&{TMM+jMn;ZQw}UCyUeoi$bAu%jW*ah0>?9Y0a7xLa6pqd+sqUNE~&x9Zs4mbB#hm0#c;HR4j?{-SHfPI&TRd zW%an9g3Ebw)7R)XF7{v&5}Yxf$j0c|i4I3Fg4^JPSsFdqup6y5xn_)_J_CwRChNpI zgXHt)wo(Z{*eTc+^>7Hj-dSt~q&ZYv_T>kyA5pZevadI$8o8N+RJNPmsN?^5I?IN+ zqL#^QK~ucL9BI&x%-BVW&0>hT|)lx>dUU<=iIQm)jo* zljh&tD=OlHuRyDU>Les1f7UMTKZ-ne>2p!gD@cQe8LL$@fz@TM8}0rxpfg3Ms7-#V zd|WOg7Ktg*{@BaSq~vtl+#3b{X}d9?~c{55@h=elOTC(>prR%*+4(w^t(&R}vMV5Me~t9}UDc}y;RO_D;uH93LP zBen!lSWin7WD#%=&FyZgj7UJ>a?NFe6dTw!H;PD>1V58>iJfzf!-?M-3JzY+58XAH>un4zLzcm+#zK#W6>T>&BFIuMOQ zzoClw;-qx%uS1z*O*8OdzqC@g$uA8@bzECac9rtXqB`=^J0{aCoO<*;8LyAJf!gjl zwiT5{33(r!u?79~rK%&Hwzc|GA0`n2@n+E14iEk*?t%$cWf3AUS>mFfUqehH_y!@1 zS3tdc@Yi$GK#lWXmP6KcqSpA(Pjjzl9!7ct3oUJ!lq(`z=0ta;7R_X~)qDRbc>BFI z@V8wuYHXQt`7=~lk-y^Ms`byk(227sbXtf3LZFzUqQ!Dd6_Mup`u(`-2CLmiF;z%T zi;11Q_UK~mN#g=@nl-DUWs95cw*=trC1qt>!#x+rPEi$6{Q4WxM$pB>cHMkERQ1ST z1VUj-;Rr={h;y&bUYx1}S5P~u9V`d~CH>QWjg=afke+8POsI9csi!W$9B<+}X)qCl(KLe+G3ON@&6x`h zWPYh+b*yQCnHjTSSI5PK5jTf{@wC4GS7t5mhrXPC9T}8-os7)-b160bb$+_R-JBah zPZg0c)-n%x z2+S06*HBq4!6!M?sF9bSqQpE%nmuS<*p*~i32kka78ZSebqT>+e-DPZJh>ECueVX) zHb(y^MIi~s;Pm_NL5lF#C1|+KF!ej~>xM=UIKcjqUbEZ0*Dkc$f(I@xH(GVy3mWuh zO>z{-;C+6WZ<^eap>jN@V{c|}Bha+u3V31=f()h}2APs% z1XT$_HZf_%_N4qK>)W;7V*m;*COJTCoJM%*9m~Xf-^-yQ&e|M|{|nrE(CVhh2iv!jv43hhW4{NGjMqMyGI1x)wOPP1)ozgdxpWb=bS00@_DcPDzo>5kt zRajXpe69G&RVDzRBJwP*{CFny4Z`9YbO8|BtZ$qUGU1b0K@8lU3f9|H>l5Sp0K(pt zCvT~fvUBS6i_~4P-I;5w^@QXgZ2qQYn>!CB*s$-1jJ1l2avV(jr5lt58DZ|97KoQy z@$c5f>#X>lV6ZCA+|xc%BF>i%c3p{9>bo)!R&~u=_QsiwBCmc*t$wRdA`wg;5?^`) zb54s(Zdn{M4Yl1tHI8UX)I4zE5Ow$O+I!}bJ_XytWFj3lyKEL+x1Z~-5l960Z5OtX zTs?mK^?$C3heZ=QU8irsshwGQ1slf}c&#~vqiy9k9qiU}{810SUtHR|UTf{bxag zpPBn}Fyeif3n~Jims00MU!*S>;fz@MMEH8XV9&)ss0u2M8-&m!6%e~tB2}%3O{uUw z={k=`I2|N-Cp(-wmL5s7fWbT@_W|P5R&>6;)ggl6?-ZP4`6w0<( z=0I;ZnW~#@5yUEQDOenVns1qV2dK~w4Q?R_VeF!OVIbm!k^sr}QIl zAQv>g;i0JKFX~nXhjli>Qx)ob+_pWk97W&5LX|n=U;KL9(TR;@XJp~s-Wdn@Q7Y=i zyzEhh@xzt|qDLh(!#y^>ZyfKitr(hBRu1pAYCHXok-iNS!#A!>)K=B^Pnd0+wLr{+ z^7RhA7)#d_2}V>$VP4)H=?k2i%Z;e{%@S8-N2MFRxlZ>~y8zx*!hwLf{LS)F zmIgDaT|VSh6t*}0bh1;_K}vG0K?0TTLqeeCLGBqOiZEUSv`>%- zQGt+6t!C?OSOxXfc{1m}gY$@lm1&%F?1NRY4fn(ZOnfG1isRoW^y=pa9Lt|rk0tKY zYwS4Mk9*P%>pphjt1B9dN~UGO@5uW51jO(R#1H8nl9S)>ZKGb1yeg_zrmMe-pY!`3 z8b}YEk1_h(G$6c8r?AB!C=Ty}bVuU0#togf$#iG2pu#Q3%}15dap-&sEKGR*NH$%J z>3?z6pVfyF^tX&LUz^TZe|wN?i2e&MDQhX+fa<#4GU)|t<=j+Rs6THye1oPtK=J>i z#e}LQqe!NOt)oK@vGWOBvohO&Ua?s(@n1^9sZ=gx=Jl1|I~z}TJR8GGuj)0*l*j3G z;fn5MU=Exyk3t;PVk7ZK8dvv=!$ekvujWwA$I^_rTL_MaR%Y zFrYH{cueX6lQPZM=e=U}Y_y^d{`fH4E1v$Bca+8kUL%=>36X1s!XtQKz8%{gpX1W8 zWw3MmpyqP_Mn-bCZ2tZanz7_#QsMT7U@7i8`}D6)v9=Us+i3|zsSjv}mQ9CQ)z|=p z^RwDRCeIM*PXcy10rb;PyFCWX$71~S&BizatxKf2Zba9{lyQ2Q$A}qudQAFQ5p}!F z3lq!W6TSUl{G6tC;39|R@SNMImN?pVWoW2q6YjAw9ySB!rw{?T^xS)zhz}uLZ@-X#(T~-X>V=9bnGbqOm3-P=YFzyKPYmnBPPs@i)p!;F0Qa7kz$$5T zDbuyqhXYgDN>BU+(Va2GC)VEmOI){8f@HSSIB|_!OIB<+=`!L-6yv}_^qKTH1o+(_ zb?El&JeNbroN{HVeN z#H$d_F?rx$IBDalUJz5N`(QXS%qXPW{8F4guxxis$bf*rB3FqH?$*CI)(LWirb3sz zCADA)aw2OAviFx#Zh%^g%l3EnxQ6zKO>A{I-T24jKzJ)v#ft8g$d+H&@KD`E3FzlG z*%Yw8>%`u&YNsBw0K2kQ(vpd)M9NmZ&Or-WTH~`&mvPbzlWk^cR?&6tY^}nBx8j>A1 zZ=_baKfBV3=8*HjHR5Yzv?asc=`w=x8sK(h#TBe+d0)JGoE1d{a35@PAu8=@)`Wm^j=*gpqgzcrbdZ?_3STaFLn zU#78)|I-l)<7O8$qFoe)Lx|lK$ zqmYQ~J78CSf8@h!%WnNZRC+tqw1_`16#YB-|vQt3(DB z8F6{!0SY8^nkZZru`<4{wJWU$)xk-ck&1lA56p1_TkQBI*aIFD>P<y>(y( ziF(-{kqxWG7^g96V(PHshgsnRfw2pOH2RXH1)lH=oz%3s#GrK_@xfC4&~c)RU^Az6e*=Z;x7?!MR1~?&a5Gr0xIAejR);d z+zyH||8%>Il76z^NaDEiT&%phL2w-7`8%z zXJSMxELZ~EQUM7OxflB^{-453^=Uv{J4Tm$y&gk~FU*sKrCGMy2;`<0{J`%T))z(B ze5NN>pB~x{nM&oN_r60r>E&6wtx|%La999y`8Bh5iBFeW8y`grp`myH{a9)2dfc^1 z>G%yCZ6CJWo~%&1xmStCH5$)@IBfS?AMZT0()kk47p}K{VV6@Mj3kwQjNp@XVauFK zx2Tk3`fksp0aH{FbUqjy;2>#Cattq{YSZVOtO1>94MYlch{Q+K&EyEyw#(hnFRmrL zUf*(Bh+WZo?+X^SuMYzT4dDf}ItsN|R87{c1Cx@Y;KnRA#$O>u)}2=P zvTq;CYWMb5rJ3yB}fjOP=iQtIztywM< zPOkcdi+d4%e+-lI{R+#(z+$;Q=qQRVvESb_Ys z8$?#aKn(GtX)a`|iXuhBwMo!-O)p92EfXE6_ft+UXFn&Y`E;!q!!l zQnLYMh|mo(cL=t%6hf6r6xG@ht#pas{*g-6*Wfn_TVL1p15IDzs(q(FgF47_L2}eJ zx7;_&O^+P%weF2YtWFi%{%8o`L0OPwG~aSXo&f;@AnhUl%8IwS)@EVhE)3lKApXHg z6DRoue|Ff`R+PqmqFj{H=H5G(3>hXzT-ftkNdIw2la3mPCDeU4SNm!4%bPV$+U$=7INs>c zc<79T4INTP;g6^f@r^CL@>CplVlU?pg)xqHGOr`T{KgnuIDyW(xulL0;z~SUFk786 z$kl$78yx9HqS~L4bR-{;QBTsetIdtea`a zwwl`y{XO_Kr~EWApn1(W%#u6B{0BTqJxwUC(e2jjFowLF<`9eaaQp|7c4Gxj(u^52(x~L(z3zxoWXnCEkkhK{GOnzX zqB$h*J^{2194wf7=b|NbcVX*bdUOmm5w2ZTQ+@p*o}IwY^l{oLMDo+ve8+1R+55ku zfdBxr)C(r8cj{L)*s1N?FC;#YbOx_`Ct%uqCRIxK5inf-_fiP_BzL9xS5^rvZ{Ms@ zsjFZ|^sA^5F@y~YE7~@Qsd#}xALwL8_JbfajyrtZuwYg>{0X)&o^L(#P*_qaEv61T zS;@16NF~C|h?G6#r=rfy9Om$TM4>D_(W@);J6UM;aKYyn`o@kkFbU!vUEQL_P zOW04{*^h-em}14yt8iI4k!l9aFk0o93m~cO4I{&8$qmI|NP*A%pm+Rs=iV_9G-F&uF9a5t^_PuPfp|j#98DMHCz_KoJR2d> z|2(08HKs7K)*?l`orP5_=IjuU>-oJB6kADbf1{vckSllatB4z?15Lw=v71Lkj4BlA=2tl%A%ZFo02+MZHN8WbH zCgor5RDPR~>iJKY1{~Fg_k#Ub`Q*zZf<99M?+%{Muq-kce;4a>XDoN18{c8hCPxox zBjU&o`x_uH1ns_T>vGuy-=Kw#ZY!{aR-;6_M7+C;r{Y%Y6W41i>0&2IjDz&zCeT+mE2c?M9JhtGP zfrZ>uN=ckynCI=dsIPT!E_XO^+&lRD*V=Dio*5GVUJpn$SH)v^^C@5BlQM`Xfi{Wdfu;AZr&M%Aud$j5xtfnk;c7S;T9`ARQ320VwvO{#)b)jh`x5nfAauN%qjp-9PFj9q+A4A`gE(UYNM$y|rZp zT7vYx9458HM`8$xRuTLd8IIH=i+U$$SxC4z85zjH(#GUt_k@Gd`~0r?{@$g?_*$x1 z@PbuPax9|lxW*qU2I&;pGX;GaCHq+M*PLsMMab2gtRD{8>g-E*WZsqcmYq7V(#ATI zlw}M~P~kHRn}#op{_iA0y|C*Bd*z~Zq6a)MR6Oo8{jkhe1hF!iRqJV< zCrYYtc`ip{LR*1K@|*?|EzY=z8R?jg7!z(u!hs#KEFz*pbF0J({4eQ$tf;!ASo=RL z>TY2Q07y#G0-*o3AOWZ#U;vPR=)W#p5QzV_s{b(Ff3WU9O#L4p(0>>W^#A)?gn|FJ z|3AY2E~mC&fKxC4;D0S701yua0RMN7iG`U7B;kKB$Ug=^{(t(JnV3NG|A+DW8vp>! U{~tztsDG-8|H1z~(0?5N56HFvJpcdz literal 0 HcmV?d00001 diff --git a/docs/boards/KAKUTEH7WING.md b/docs/boards/KAKUTEH7WING.md new file mode 100644 index 00000000000..de9014ed5f6 --- /dev/null +++ b/docs/boards/KAKUTEH7WING.md @@ -0,0 +1,10 @@ +# Board - [KAKUTEH7WING](https://holybro.com/products/kakute-h743-wing) + +[manufacturer manual](https://cdn.shopify.com/s/files/1/0604/5905/7341/files/Holybro_KakuteH743-Wing_Manual_v1.0_C.pdf?v=1693393206) + +Note that this board has two i2c plugs. +The six-pin plug should be used for GPS and compass. +The 4-pin plug should be used for other i2c sesors such as i2c pitot (airspeed sensor), rangefinder, and external +temperature sensors. + +![KAKUTEH7WING wiring diagram](../assets/images/KAKUTEH7WING-wiring-diagram.webp)