From ddc3819d6c463958df865d99ade24be5856e6545 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Vision Turn Speed Controller Added toggles for "Vision Turn Speed Control" along with aggressiveness for the speed and sensitivity for the curve itself. Credit goes to Pfeiferj! https: //github.com/pfeiferj Co-Authored-By: Jacob Pfeifer --- cereal/custom.capnp | 1 + common/params.cc | 4 ++ .../assets/toggle_icons/icon_vtc.png | Bin 0 -> 36850 bytes .../frogpilot/functions/frogpilot_planner.py | 39 ++++++++++++++++-- selfdrive/frogpilot/ui/control_settings.cc | 17 ++++++++ selfdrive/frogpilot/ui/control_settings.h | 2 +- selfdrive/ui/qt/onroad.cc | 6 ++- selfdrive/ui/qt/onroad.h | 1 + selfdrive/ui/ui.cc | 2 + selfdrive/ui/ui.h | 2 + 10 files changed, 67 insertions(+), 7 deletions(-) create mode 100644 selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 6470d7e..7211842 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -38,6 +38,7 @@ struct FrogPilotPlan @0xda96579883444c35 { slcSpeedLimitOffset @11 :Float32; stoppedEquivalenceFactor @12 :Int16; unconfirmedSlcSpeedLimit @13 :Float64; + vtscControllingCurve @14 :Bool; } struct CustomReserved4 @0x80ae746ee2596b11 { diff --git a/common/params.cc b/common/params.cc index 84f1d1d..ee12627 100644 --- a/common/params.cc +++ b/common/params.cc @@ -247,6 +247,7 @@ std::unordered_map keys = { {"CrosstrekTorque", PERSISTENT}, {"CurrentHolidayTheme", PERSISTENT}, {"CurrentRandomEvent", PERSISTENT}, + {"CurveSensitivity", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, @@ -261,6 +262,7 @@ std::unordered_map keys = { {"DisableMTSCSmoothing", PERSISTENT}, {"DisableOnroadUploads", PERSISTENT}, {"DisableOpenpilotLongitudinal", PERSISTENT}, + {"DisableVTSCSmoothing", PERSISTENT}, {"DisengageVolume", PERSISTENT}, {"DoSoftReboot", CLEAR_ON_MANAGER_START}, {"DragonPilotTune", PERSISTENT}, @@ -419,6 +421,7 @@ std::unordered_map keys = { {"TetheringEnabled", PERSISTENT}, {"TrafficMode", PERSISTENT}, {"TrafficModeActive", PERSISTENT}, + {"TurnAggressiveness", PERSISTENT}, {"TurnDesires", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"Updated", PERSISTENT}, @@ -427,6 +430,7 @@ std::unordered_map keys = { {"UseLateralJerk", PERSISTENT}, {"UseSI", PERSISTENT}, {"UseVienna", PERSISTENT}, + {"VisionTurnControl", PERSISTENT}, {"WarningSoftVolume", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, {"WheelIcon", PERSISTENT}, diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png b/selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png new file mode 100644 index 0000000000000000000000000000000000000000..8218b456ccd690092126e628e7485e21ab40c2e1 GIT binary patch literal 36850 zcmZ5|2{@H&`~BYA#zxpEWR{{b79kn76qT`3QZl4aNrcF_%}OXr2_emcG9-lvDV3rm zDVZuNN|BJ^zaHnD@A_T;=lZ_ubWXg(Gu-!D>t6SJ*v86?pI3sHLZR@lU9)Nfg+jw0 zX%q$*{xKZbJdS@*eK(k`q~t%B`a+?|QP!?9wheUb&*E{gdiQ7AdXevf#KjvG5*N=- z*L1&Ze)&Z7v!aDd=i6R9zjw`=sNCf%cU{&JI+@$GM@}o}rQFbH_(1U3^!vZ7t+PVU z_RjitsN%j+*c->}+bzM_df%42|F|8)7^P6?G^cm4YHs~0a`9GSFUw(NH0bU2=o z=VIhiEv^2H)YR05=FuxE?fJfz3fii z%>8jHOY;goVGZVb*?2YmIDp z!Na0b?6{bC3-&a9)!#oqZpL|y;Ql&GwY9ZPI|j#Oxit09wNq|vy}go8xN@n&4!2c2 zoD|K^Jm={6od@*UvuEEsbNckeJG zSRO*Rkki%GZGHayxmA_Rt>*@_Dc3VLtYI)H5pGQ8*LQdJ8x8Zzwh#Zth>4Ew{If%o z^2@dFK@_*3nqzt35~C2GdTxh4hco*g&%y6h@_1k$G@LaEPD@N|c&4`fo*z|ZSh2IK ztLtvqWRO^pS$%n54#oLHrg1GcNHLpz`t<1&6%`dHv$EQ+-w*vevM+*unh{?${`;Fw zU|?WB^H^}py%-w%_C~(LcoP;gnh|s<*=xK{DEwQ?jcpurJ_~KRA2L>m&GL7t`hg&` zS)7!LUhZ=oG*%8Loji6w#tjUYCa&Kp?u0gT_n!^2yS&yHDZYiDO?6gb#v zymsx{`k~$z*?Ac{_tL{FkLOjsX>Ohfb-jrrsKn?ti0NV%Qy46&9Hp;Q7~9E{zIEG{ zEf%K@f=eb#u@jB?K1}yuj^5m~#&$g7HP~n9v)FF{tx9oJ*J=A%L0Dn zWQ4l7xGXYe%8rV02JR5SgtTg99OGj;%Tw{rJZ5H(#-66BZhL;x)xV?s(2yeHn51-M zl-KCnw{LaK&CO+tyo2NyQ1RF-_TYnM2nm&nIXO9v{=dJzR{q?Op<|8narDOZM%M|9 z5Wk{IOupOZ%`FXB)?Oj{&-5G1O&o{iD4Z07&(w2Ri%v;uam@bLS2yi-dv%MvutmsX`;lkBbM<; z5)u;B9m~Z}WoBlcNJ~qz-?(w;gyopA+Oq|?pKq_gJZ_q&oZKE)SIzeN`rZ87M!x68 zF2{)^dBUeJ>R?xa%Q-pj5g{x}s4wqaDz!}n@l-Tp+1|9CU&s>y18ujJ2ROXuRsHDr z)x>WkFE5`i7aw5pf~VWvh|HzYqhs>S9DeG81q)XDy~@_ve>N{VI-0(Zw%1K=P_;6> z%2`(x=brU+cSH`GQpQ-gc=6)9Wo2aznaf{owzaiAaro_nJ0XhCC2A*D&}=AGzo~RK zWf#3sCd}R4{nQ2XQ)+wPwBFk$?PQe7+ozfL7}EGou53J;ue(AXX#9Kc6J`aA5Yw(8=a@pZ((gIlIxf>+I1z? z-;@&Mq!LA0c2KeC5AunjEGa z^}oR*{cr7TZT+&9hiHdr)TNkqITJz82+C6)zk#+*F339E&l(yUzMS5(diCmu+2K=% zIy)akg(25nzuLHmtfeKvT#+BU#4ne`6W%$Q$-nhE#*rv3=IuGT)A@P zi|R3o2+~j(IcE+61bZmS6?(Pb+eBVq<)!ozcB8{(Jy)p`$7+nG^5qfWaWXaF5(b0aR&~ZPOrKP39_U=esHf0-rp(-ssTA$5& z$|(N*>sMRhp4V4j6KJKHTDA9Nc6N3p*2=P}sc8w_IOM@Kfju+NLY|6`#m=M03d$Nb zzj*P&3D-;%V5~!gA>nGduI}jk_oH7l6qp?Ea9*lL=EhF2tHK#MR$*t)o?X#P0?nE= zYdR}Auredh+S}VFF~3u{&dI9T$yFKAXwzN2PFFh#C@RjLu8QE`28x(Xx6I4W&wu{( z>E27HPQ3|>l>5NYlNl7Gmy&~!-=O>P=~|kwuyD%M)KnP4M_MG;Ac^6BM+S!80z~9v zvVx2saZE)}?8rmCPLC^JZj!UV{C;;Gb1+$6w?d^B$Hf=Hx z+o{qi$*@_KV**IItIY4qJY_XCwGS69Ge)FW9Q0IIQBm>x^|5~A>@m**mDvZzbgUssq|=&4F934Y5JRmMxfXloQgR{}d(fy$jD7`ZPR!`q%uJ4|~5{6-PwLy?ghr0x(E?{rXh`Gcfhn+8+a%Vy#Sn z_;NZgW$AF0C?@$VPapDiOiaw`%gB(wJfjaeZ{51J?dqm$o7?aCe-m}veT;*)lfhVo zc!%rl38`%Q^Y!(&9~t`N2gw9jm`>Q00Bsr1(E{2U6^mHz`}x`A&dp*wbIDT$#Kwo1nMSffa`wWm19>x$Z9jR)nOs=a!B3_Vx;w#jDf0-*dz7Ojwwl+2S1Gb&8} z?$Z=aqlm3-q|#WOPW0m2w}aziV@(Ns;-xh>g-`x|(+yyvxM0C{k?8QjdP`bgtdVEy zEg=Q7B&pqd_UN5+e{t5}%Q_k_<=*pIwVZTui>kJ^Hm-d)L~30d?qJjS2LyC{?&>PY zkc$_bRmaHHWV4SRwJAF7W()03q-WLE;&k+(5atVJjy_3O7v2&C7MW+SwFmUCkdmGLy zvkKf5e)Axq3@t4!nqD3ELr-OgJrMiLxP0Zxq89)fNl{TX{RA|-IK^7J(>YnMjyls3 zIHwimBxcX{JKE6@N~6(|R_N-gC@{L0SumI$@)0eXZqII=CSeW4FuAig^_pm%`IWh| zDJQx)edMb2Xwuz1Jql#TW1Z5MX=!PxY`Nt;1zNmv?9)a{SR>V=uW87PI_g$o-h5&Y z-qm*iFM0mLg$wrC8p%8>sZ|OK#kuIERupN2pzrVR@?Ac;sB+6rH#aw{Y{T}i$gdYB ze%0?c0(Ck!pPT(c~68?xdM6Yrfn zcdi@sz{o`B;4t#-q4gjRe%`J}$~|t@%}OLlltRnUKX5 z0pH)J^WO#N_~~kefef9Q+8p&jme5>&iNGIBw39X9 zM;SO$(1r~!EwRa#E>~K-BaX)Ay_&;A4>O@mrc?xCb7^C<=L*jbdNhDtcC=({{TH)i zZJ4)l;2R;}5L-nN?zW2;AnrUY2`DdLYW&WcplM0zny1(Jv+4{8oVxdRdh)fQfq|-y zj*jf}n;t>2ITVjGLPsgIRz}WeLjvf53i{(Mhm}?$NrbMz!6+LX9Aqq>WNeVUo!*XP zG4Jw&g$8iXS-e$vTj#-vspRH=IQ%u~O8$Et)) zsfc0gLoQim9f`~^oOWaIQl<*Htluj%sx4dgTll2X`Os}&DnEewrZxng0``0V=n=QS z`p2gv0IN6U9ndb{D}#+OT2)m=xp(`=>YG;@M;ZWYSS9xhyPrN>SIskR z|F%mPE-Zk)ciQIRNmZr!0BhgQ0>KduF1qRZnY?W{$@d5z>g$cXvDfxYO_Z7|py*Qz zmeh55*6+W2{Bi`p&W}$LaO2mK5i#alTvS--kMf2p?mnj) z0qrVxr>okcMT^pbw2tD$c@}PsEVe`Fv|O_$p8QkDR?1_5-gX~(^^-q|fssczCVp9g zj(o6xutsih1#MqpQPE9Faq+&55j>Lzpzvh)gDfx?52s(g>;G-@pF~NWdnH;2{X7Ss zoKzNTX>Qgkv5gWq2&HZy93g86FS;}`p4;H$N~OhE^L}I=mjbyRlx$T63McDnX~Z1B zZK{z`Wld62($JNv$dd5}L`#{BkqfTf^JfEr#c9t(rdWf%6d{dF%GQDKugzCB+FmFiQIsT; zH75;I{deu$sRy}gnF&sH+$sG72M&0>Yi)g28a^E+q~7yIzm9~}?P3$U8^$Z8eqoSM zFY)2$t1;f)dfE#qXWZxt>?(DhoDahUKiXz}zXSHU>BEN)9zGHwVa62cT^U(fq1UdM zq(Ru|Edo}&4HoLGXMx;yBLmU=@elp05J+^q-j~*ch01RJJ(OV>7H&i7bmu+tUcT`= z4tRQb*k4aIQRpvXB>|&*)l!`jWwinH30M&YnjHIa`plU#65QNzZ;2A)GbWOgw1&lE zS0Wi27`X-lP6j(Ubxg|0$h^bBaQgL1XY^bW$b54F=dLu7oI=g4jEszTgznvvl5ZIn z1^gRq%r%(~+LS`J%-+2#Qu%uy=Dm?dOzr}Nl}?!BAhH8|?Ko)UUce~XU$4|h&z&Wg z)Z`LWna|$*k(-O_Y|gx8!;fVR#xJeR7&t>Fy%?~E)8pw!-0D3^#94Cj>J~2~m$9Im zm3aV}RNZuF?3p6etLMNAp0EPZ6S4~_^wlyGOVcQN^FL#I>Rj1!C{f0hL~yBcr+q&^ z$xBE{NqG~>`{m1*-Kv{&kLMx`G~d{E=VeonNLWnF=08Bgy0+lKy{D`PYr>%)4Q4M7 z+^HQQfOK#t2%K0lK%#Gz*7oh~_Y)Hn)xGyd$++?qn_F8~E+t58;&CU~{{GE39Vw1&-Pep|VV3o}mX(WV(U`S28^+=z3FuJ#~p**g@ypWcyyl;WESNeDTpA=v&(>f_2OQ0mTAN-|7?8 zS^xPPCgAy7NGMTXEg)*hwzamlI#z}mMD20Pu{I(a+I}m!c@n~`_R*;c&|IImG_182 z4rddtUwwl==-GT{$dsCxJLnwoFFRg%Joo~NaIyM+n46=L?zxM@v>G8f4+_;-gmvu} zse7m(&>YrGE5rbUkz6`4aVT?*?#*4`qQd+Bh@g6MLD&@Z&4-(t+wUOf?6bC!7w>A% zrf6l$F{}saM6b2!@XtJ6j&eyBGM?N;S(AiRo;SCNN}rONy797})%Xf0JG(cDP!>As z{68EsGA4x~DoHuW$WcUG5*8H|t>f7PBr@Wun<$_pqPBODvT}gArDa7v$yRd+SqSm= z_CC#Fwr=C<(}(W;TQ!*yVyHXt@>9w*`D)xoZ;k(&NdLo1htT8u#+6W4Ae(`gH z*pS+%e1MDRUkzca4G{^de0fy-*s3b9HtmHA7cwrD;T763```H9-SFP9t7J3or_mN} zy-AehXHPdbZ9_yn_F`G2AS{N4?+3X(M|g0XgioJ8i`F9v_Z;|5Ab1oP|I+%$k4uxx zMf^@%R#jJbC!_GU=4#<%6l7!&H=?18xZ93Vwtpsypx@Ql8#7VL~X z%v!l}Wi@cmdS4mOZ0!EF>aTk}b?2(D`sDKxspb=4H3#YnPsIg!g>z2ewuFmb6cPv8 zVh<1dUEjA}1OuqpGQE!0eIb?|8M*Qe=#c{{R=$_P^L*t3VJywbk(0fn-B*x|OY~=K zd>l`Y06@qhdvKooPxV)Sz{!8Sx|IB#6f3=VM+)TeFt(#Y@he_P;g(qR<-5&~A(iFc zxOP;L{$MB(R|zrUPwKN`0WX;E+V#mb^T$;C`}ZYYZwq&6w)@77j+!`wOqh@S==hQF zPExPwMRoO#b)KI6_V3$r9ywwyZv}YC7r607ts5I5j3iw7i;1k(Tg`~5k0)cOj@()DM9Msm~oDvW}rU)0!~jm<>bhj4UznRS#4 z^Ba%uiWN790Of15JXlB&QBhGGUfUacxS#}=E_rcmmk6CCYpz-18Wir!gcQF7vN?p< zDk2q>^T3k^(vgL&DGNAyxa-+RQsYBh{hiY!Xm$1d%Sak~+YtZTsc3dYM1;%1g9k;` zbT8p5uKV^S?o*=Xm$J+>=p^iiLNxy76Ep*9h@Bs>=6<gI3o2Yq76iGSP&%;>MVDUgk@0~h4POVppnM^S){eVH7*VrO!K(~$?txCevOAy zZk5rMwg~KjdiP*$+==OAHmX_)adD3X94szcq(@o>B-CBt)_b;qedK9%b@hhKjwzy3 z7eL83g#ugA;INdrd^@57>r^^hmu{T%I#tPfUD?5bwmES_`x6SAAzeh82x^_SbO!o& zER}tQ>XO%cYzA$&)QG+>D*?XlJ)_q?2%x2+BI`HOZ||A)Dh<;8(Iaa6L%4?;T=3q( zBoL9hg%beGS{omYrkmyETOS-fd@DTgm(2Fnq zO)`5>ZU~lQk`gB<*vZ!71y9@CLo=>j`^Hs^XIc?J;EJ@kcwj7~LEX~@_OAbnD0WGN z6kbEo-U%!`9$|@%o#)N(eUd#0(4S4jylx6x=1=~%I|AQ!s7v`AZ}iyY=s2|nX!EC8 zBH0l{R}~x`AD8j+^b9dxncZ-40B^A=X6PN+6L$R_l^t;njdD6ucKRJHX&zO1;SQ^ed;h7#cFw0}HrOLnSphje-zoAKv2`IIA78@71LH(^Crll* zxKK`Dv4Id^NF^_3a#L3Pdo!wMwrti$$HL)l6u zPE=ZJC!Q8`^~q|81!qL85moc=+_@9LVo~%qMqCSSUV$>_#e3VaLC(~Cw{um-&mv@; zaB1fhIFs|!2_dCw*@NXY%xk-he&C?ES$E%0Qrlkews>?u)JJ7<3hrD8AtahWhCxI$ z?rekL&Fvec727OvjO!49t$L1x0-Ij5Sx=4}yID$8I5Ifz-2{Hp%IR)(oFiBQU#ULMK#Da<5X#9XT5B>>RmJpkCOx5ccd zZ{7qC%Nhg?Gr1-64W^VB=LT?69B+e9%&`wc)~>Gs zZn4_T-551CI4| zQ8iN^Q9C52iuyf1gFlTTuHE*tw||?8K%QGMJxHaUASe4kZ717`ZsU6r5hw5D-}8q5 zCA5<#7!5~#=9qbUCN($Pu7;>~IwQl{YmdXm=`f^4O~9yLKtJLBGAT02K3d>04fbsNgd^;LdsCZSXdAu>jl5!$7_A^vC;_3a=BC5llr@{V-xl4_?T%Rc?xs`a{ZB3qsU}Ag6S<+GfABaUH;Y0O zCof8!w?-q1k}pFcD>{Gi;v?e~;clj%EzJ%eG|iC~woLX6iiR+30WsC`m4dw{Z5x#s zzWyLME^av%vC6{|sb#CRAU!U%3cFUHCNcnX=q>SUeEuI#r=EVT2eO3~ekLn1+WMa7>R@IEYm8>kzUlJyuT`9qg3k9T!6j|gN6?}&bVwgIy< z1{We#wWCUnMeZ-Y6Iv3jAT*pLS@EqdceNOQbA%W8UjELWdSLzU*uzeRdKN|MAl;i0 z`4&k2K|!bm@~*p}z4R?^S~>9yh^^Cz{YgJ@@?=xq>v>X#Z{6Tw?kGGsuvJN2{bttA zF>gO1IUiDg5y7OdN$Vgb_~&3vGXMV-v|tVVH8eEjJ%66Ea)P6hehqmd2g{VjK<&f` z_a#NFvb*)J@1;|zsjs#cPL&286SjAA?l^$JU7RVsWobMch4Lw2i38rz&+6P%2{!`0 zohASD5@d??Wba)Ucm%vl!R3x9KN4*zEY~~rjrq|M`q|2pFSrKHhbi3kkn5u@Kv~v8mexrYXAwGn=ywC8pay$BMVJE30eL?j&h_k9BxslWcZUb-` zysg#8z`y-ixjAn42vPQ40olYVcr{z_EZg+YquG&1+lxo@uIR}JLL<~;r$R9)Yczq3 zDr9W4uy|$;S@U znx=|(EnT929`B>hMBII}Zq*;z*SG@;j8~4(3hN!1&DLZxvAdQ~Zv|}#fp^*(tP??3W@N~zChwdc_TH4wz z{{5{53ieig(j{053sN+&SH1Yw>7L)0mzUS)nRdlX4(LHB`Z5V9pt4QNhddw28^NE2 zmyYpMam-J4ckfW?z5cDQ&*Sr_Pm>Q#JAY!h)*YQ2rJ#c%(l?a2vg>jm&gX{!xSa5! zBg$@mjg2G$_q^ngmK)3>xJA_)R@?p1PU1ej>$~Mr6YEfRQ1xVDCyrsu53ryWN$(ra znZx^+u3IdAvFRd0YB3Q}Je6 zQeq^%IU*qDB^}K6>07dvX?A&w&hBc`+|%|oz~7+&q` ziPkq$mMYZKy1joAfO8y(>voM$2kN0*q>|OsxFaPNyAJ%J$u-k5&)HZmHJ{>E%lnVL zx0VG_CnpEQt8d%vE6GJgLwY3ConPhQnTaRUg;3h1yAq}MxJR%mwcu)JVxe}nfEDbk zo%4s#5kQx@y>MgxUzT*Geh;X}+= z?=u6e+y;L92=^lq{jnl&c6%A}?-!HJanDDH@^KSkqA5kZk{HXDFJI2%Kc^gZYg>Q+ z-`CRmZapvsCFA)$v>%Z~GIK2FGYbI0%CfOrh6n;Wy_pSx*wib$r8XvPEe3HHUG~&v z;Lw2TaOm$25CFf<(fRBj0-(&*jA;CIW#<=q3xZZ|^m z`DC$+L+58h6eL3Xv`!sKv+JFJ z#0o`JEQI>!BcS;~_^Tz*G#&ufKEU&rw4Zz zQFFQaRyku#x&7-k#=h8k>$r1|Za_{?i`HzbPx*MgVh!}9LP%gd zbLfO7KZgjUkJ*pl4Q4ScLnH?5I*#fetoR-3g8z{9k=Yau@ zSYdD=@1O#;^YUYv+c!#zi!)*U3O-9NqD?Mh5Y7zcV_0PAyf=&;c-^tC?09esmxe$oD0wuq-ypSr4Re5#VZW}5bKe`zN_qf`DE z%&)G_&aCme#1Tj3MT_dO`&v2=&$2CDCkM&95%4bQ>csjVyu)9=5lijmU(|cgNUAOR z%y;hLsu@YTG5xFVN0c>P=152cam9&xc%NZ=goK3j1AsJk8J-Aj{2m2Jdp)LzW1-12 zahHV@am`K4g;q3@?j&VpldTFBCJ)19n2ES)`*NjR)gc-LOX;5O?i9Qt|CMMTl-LtM z!Zs5)cx9NQZCDYHK>YrBNh~7#HxoBnivLeQ%{!<~pSqEsAGUfkUw;G}I--i+p#l_% zpV+>{8!gnp&e#Ia{IPXLIhI6*X$Gnal}fiM z>jYkNx4CUFxS|W$u>2MlHRZ$!KmNctc|5a@ag~Tejy=OR z9Uzp>N@{s+Dbx-l?+b`Tb^z9QuM`aa=qHxk6`zF)J?2uoFd0jcxMI@Mr{B$#Ogxmu zX7E-A&r!HQLU0*d!pdzd7eLN>0E@QX>_Xeri*bgiJ)P!`U&*3}gogf6w=dkeNZ=1V zfIc|XvdJcVT0~(oMVW4~qT**yqNJ?1{EVv5A5yXZ2`zSk?O$5x1E^=LTN4k;*gVK> zvxxilp%7QlDHuauUx0SWDPRCpY9y?&Y?*Wz3_Dw{zM;!yV{LB}cPx>iHTe1o5e)7X zUD}h#wjuV9{5O=oK)d}I9~^2H$~EfKFrCs;I9$efo$H6v8e|jOxYc2zdkc+Na9M|h z%4rrthvGBg+BJ))#iP54bI4nF5n@e0s?O>&iyLO@j9MTEa!~g}j4|UrV(LvpohXvv z_(5*NQIg_`r9x|g#(%iTp(`CBy*h8BgO+ixJH1iVxY*em=_N%rdd9Yas`M*TS0OdU zK~m70=$hr&XUV!uv|L1eA+yuOWHSTxXD0O~gZNyxmwL++1#J7M@fBDH0!hzK(?+$h zQ4ZN=l2L-{IwPp`|IyGSo2DIbY(MnEE_vbI7s16GofAPEBL6BBc=KiyYxzO@{2Hs4@ziITB5BzqOWEETdXCM zIWjWxp=ifL-r^nE>e=L3KHT%a_7`>HJH~u2_vIdag%9nzuJO@&`g{lk=<84>l|vab z8DbaEZ~-BoIeUZaD8J9O63G+0{QhLv3zWxhm6V(vLnPba*c0s&sZ@1`Z`I(Hm2_6l zZW^|@kv5DhaTm?B4Z2Ak9%T?>{*l!&kT9iCL+FiaCt#DQBGrzEm&SuH;79H|Kql!% zDIj+6snV!C*(?Vps?O}9k0V#k2CRE=M zY?HqQ2Sbc^4zuIfomQcL%-Ws{i1%ohhlfYJLM1T;!qxa=9dHe*HdOi<6UqRS)$zFD z*|X80HLuC}Bqj$7YwJfX&exV7s@d{^WIahG@rUv>sOP?*5C>=5J;8#!f{nh~=SzeD z(?gM(OAvD#X~`iDLASP&K;5l!@n_ll;6+j{7V=tRx{e=~n%U~{nkQI)fm+s)Qk~RK z5N;)YU@rPhTv1)10sQRaF&##t-yPe$1qahHJj$HW@D!ES8pZDqD3_C@x=`~p`{vCb zQ^W`P@FY!7j zf-Q3}KFLYn02bhV+wKOi{SX$N?Lta^vc z(P%pN*AyUFi+70Sv%|(YouBlR_lICi<4U_YtREk}-pOLfwEabigrD2PR-0}{33w2nLCtd|%`T;1#oi#vtlY54VpKRpIEAAT} z(=X4@o8^&G1x-zn{7P>1wfYCAxzUdo(||x=sKTtBHKKC>Zja<;fjbsPAWrH!B%gsr zd)g6;a0LuEHdl9{pk#aWK?Ff_!q&vx5m1U~92`-BbNO7)C=%t57}73a`QHtDlq=*! z$vK|2ykh*1{{39!w60J0M|d3{cG9|#8T#Qti7`)B)gvqnn%-FhbSQajesvxjUeKfDm;rIwP@qw*n z2XAa7^7pZ|c>wXoI0thaZyd`V_1y+JD4nEKKxdstH>ZMEYu&u;UWINj*Rg>Nz9}%V zY88{D^E?QmkHOURg&-F$P&H-Jx7ulhO$6RQ+X&gpK_b2v;$R`^uJzuHN@l+ksU0~4 z4QL)Zi`?5PFXkQ7YdHclhQ%)xvinmLeyqOB!M)K8eq{bTg5n`2q zC18((q1-3pl!NJExiGi&`JaSjAg;GW8+yDS58)F(#{u$Rx-b3%Z17s$nFMIRsg}GESI+C9Y`O_=jotF;f)v zCR$bEcUy=H-@JwYoJz!n?B8xVOSy~HoRQx}u?wzUd?{#GoJ;9^VqtQ23*%#NZ!g58 zJ>AS}(63IWq^#`a@ILrkPfz-JON;1UC%UU^57pa>lB7KRM}qJ_a6freXWE*B{~8H; zTFQKzo6kI7u9a0{x^0(6o5dH9J^VQ1jeUCTqBcfE~G-#Fy!b2Hg! z&WiE0h4E(1rQPnQtuk<&_B)}-wAi7cFtA?MUvQObw2P0NdU2QmVhhPnu5`VC?<%C8 zv##)3OQowhFcie$WuL_B!Y`D$J`xZte$1y7g;t9Rvd)PcMT)~LDHSp%*1n^ME;GN1 zX}CX^;`X4u{-rF!%T6oeFi#R5gs|-E=eH{1_?qxWUUv|o3kXz~&Oa1iEQbQrda(O> z2v~{AF(8cd0PIzDcMFI$UG#6%Cnaw(6WRt6FBrwA4-zkiAc~%pr?{D|6MMSA~G= zL8KN~xuPaQXTb+EV}*qu>?d`F@$5bv%1I(*Qxo`2krD^_+z-ws+10q?R;&lGZv?`{ z;(RG0G+>4{VTC1pU^&@ON3TQ~ots~Q)m>el84Mct%PQ& zx~J3@#{UQU68-U8NDl#QlI~#Njb~;(G3t=uT3%%@X_*)MZ_{DN0X(4p8oOYo6Kcw72HCm~A;N#=tq+hf*PF?XY5Ndr9 zcLCoC7RkyT_xzPb?3hUA9zI>$u}ZHG!!Eq)-Kh7&Z@`P6&3nSV(I1UaU!NJjFP3Lp6>CC-s}gFwv(KXS{#bYXS{n)I z;NJp}(^wIH@MD-zW3xhk1QsVVqg4s6k$)bhE@8h#KpFOIb=q;TJ@BHw<^k3Cse~J z5J$K)cXaVf6C|9YKgu@Y`nb{h!<7Ovhjg=4svHV z$_92&7~Tlp{AIMq!{D)O_{2{$q7%%)b))~gE~U*i>Arn8Lo~EUn#lW7TRoew=AP+m zKMb+9aiCg#;33xjSb*{ev-^<$ZiA**fvpCYZ!2)e@gv{2ST#s*gCAq_O!ecf4JhUC zTA(P21S@+Vn0HFHLno}73GvAM?5c8 zt*u)sK_QRxqEBljmDT$;qE+0i6RLNwBJs+bbwb}2bqLcR(~JcK`$CG*@lon8M@a)G z>9d#x)Nf-#>|={TW!DnFH?JVk{?Yca&oV|kYZ<7^P(RD_s6r=%em9VlGH-eq5`jsA zsOr{elLS?X%X{Ag*53NPhLy}lrYqCd(rPY<5!yDy8ptI@{8g_D-eR7!e6dk_JU{&@ zDn~wg>1mRGLWISa=Jlcc@UGusq%Hs!Jm*Q2+vzAMI7g`*T(hd?ln{#DKT~~9wPf7bfLd4{-I$qOzo(!f4Ct+I+g9=Xavb~aI*@)DY@@-rDwNi& zHx$IEL;t8nh)6TCXVme*58fruACD zuQ@(UHauz0S}34evYY54ccF_IIvhgZ|sO@`O6ZUr|$T7!Ycs=oP9RZ0ew~IjVTb03frISn|t1 zju1T?;_(O{gP~hDvd^)6(D~hISgaSr+Si^2?d4&*q-4ecV=8-ym#Qi^$U)6~^v3Fp z*6tlJ1|OnK_|iy^9;TyyY>sSkHTh~s7f}z1U=@KLh^mwd($XBKG=IK>7fgD}4)*8L zn(#Xnt$v;wvni$d0+73==B22n(QV|q>*9jgB~AcZwJs z_!!>-KZDq@*DggB&}QBOw@rtwOG!^ZG(=pqQLtjWtTxJUpS*LIC(9sM+wCRjqqoo~ zF+o5^3?m0E(R|}EJsS!m#cS>g(Te+OD$mi-B_zF5`4!KW9mj$0(;7UUUuQX zztu_-S=l2p^Mxz4l2#KRYTYN{Ij2BO`*iozRhmZ9C~r1Vp*xAIVG+Sg_d2 zFlsFa%~PopHB|VrdgC--K+06pLpOtb=9$RGNE(aT{6NW_p}Tzfjkh>MYK1&`t36S- zk>-gEG$HaoJ0@F9zv<4Kw zWEkEj|05gDnPVhudjL%HT3Lqe29s2O*%?%pUdBtH$7NJ#N2;wmCnW=KZADs(E2lYxnqeY zJ~H;S_Jas0{FoB;KrbKFp@P!rYC2qcT-`WIM6PNFZN_!!)*zweun6(O)DmhLubM&F z7oh2X9*%t0;w~K1a(pvoXmcRY)awGR9QgQ?Um&U}PNiq|<{C<)^1c;H#g>B;gL}h} ziN$sg*4Wd?w}rm<^b{qlut}|zl>TSjdTfd@!sHBvJ)E!_-5R%0?~=xS`*2^uC!JBR zW{k4yUoi`r_?*4AZo^SzTz zkTO1O;>4cWd`vlppAAz_gTb9ZYMyo*Hhd*t9hw~{^I8Jmv#=B&UT#fF%U6KWW^u=( z-7UAJI%Y=orcfho@HeU|D&@&}+myQRlHE;Ocx&kR6vj;Ztf>eqc`lfgx%d3s%a>bk z6c!4bFNsi)K~=_6&Ku!Tlh{v^6i$(1>;?!8O|7k?v^bfQ| zR2++yUm?!i{^#xy;f&6SxvPULcE982q)gB$)FV8zupPY*99Wv>*o7Y+KwpZP&to}3 zx~>_eHwh=`9y-K-e+Riv^y9+!-Ll-dcerv!Kzy(;-WO7@IJ8gF8FhBJFWFVo!s3P@ zqm8w-wL2X=GqbYZV2TH7FPOy$%%<4da9WZu4RhoR;H-QFK2)3j^Q4iTH0VnsTLaik zoE`Oo_}U?X{>d2kTQifFG<4WRCG-txun@W!+0qr&j_!+7C z8g#-84;!NesN3Z-E0#r-WA=BRP^LHPgb&jxX7`Co7D`G#|ITN&$SqXH_`%GpA8<;Ae%S|jfdx>@!`_Ja}NMV&>`#&m8>VU!VCg~cn zN_8C5hb(N!!5#2ko-In@hrA<~bTs|eC2_N}C}{MpbOs!iB()NLGksDLFC)oNx7d3R z-ZE9ugBr1)PF&fBYHHtU0&QK zH=-1};9f;_6ph{gEYg5E>O@-!#QPS~#JFzfd__e^>^$ki|2%qU77Jlw5J$lMm)Fm4 z@P8&r&3uFm53(B%k}$uIf(KDiRlO7oO}dA+6pi$MVO3&5TDR@W->EFpmTn6%CZgotYB6E9#p5)zwLoKS^?$~994A;nQpCEZ&v z@UCts8Xxl@U$em{FL)m2#+Yzm1!o&`FqrfusFclo=F7Vp9a=f{V3C^Mdm=@#Y05>! z^#gCMMGBGlAzCSePvJ-=JeREc->*jv(Mbb@ji8Ui-&mnW$ajIm0x3h@RLabEr%=i{ z+1)pbAgpB6MYlswb7E*;2roCGvf}L7;^Nkw-rpWKuD{)vF)D3A?sFR@pdHjz9`8q=;0Xe;!^{AqNgaUxGeW zi>B^uLcWP5#ts41Lae~C1&onQ#T>ws&MRLqjqB2D|hRTqkIaGvhM2XaGP$*MLp&5lJDn-(u0Yw>_q{+}A zB#jh>Oux0=_r340Pk-I3uXCR7^X$F$+H0?;cME}EV9NTwN2sux@%hHLtEdD!n~DEa zj%0DXCr4og^)r2xpU^A}%-(`mPZp1t6DZ7JTpU+g7ifuXPqBZoaEd&tm>jXvi4F{l zld#(A4Zts3l`c3&SO*4X_l&b_ea@%HWw5(BJkcmOU+$!}D1cse_6bb5a0JJ%Zb7Jb zl>>`9bkb%$Y+R1aa=MTZ7vs_#p3I2=g9T6jz1_F+T&bw=ss3dmubt$(IBol4Fi(xiNVD9dVP(TO) zKZn^BmRka^iddNbCBf`X7)9Sj8yj-L^)}3Q{b<8F`@>C69y+BTcD!ZI&KF?AvaPG@ z-RGyylXuja>^LbUuuA~-!S|LHgSb~e4E7&qBR#flKtbwjdJ5d&(}ys&5{_HqWQgev zL8HR=Qe!Q6a{eGWTjdE;4-rF%V=hszI2y9EEvg$x9?+R2^LEbYw&%oS?~^xLD+Q@` zuE~F-p{){==97Nv9;6jJHuzt$8wZ}+ZJfem4zFtn70Qt_!~@N)N1vy;6wALr^(=!2a$+?xCP|6Cqz8aog+Wxli_^C(hQIdALSGAV zw!H-6&=0n3j=#U`9K3IeAu;{@IEOYi$J5}lPpdT0`@)Y-6xgO@AYC%e`PqNR zUMGcMeq**whZMiVwm@nP*3kD3vD2dFU-suTtIxmVgc7Jt zo6oAM^CR}Fi(+Umzx=5FN*qYPts7HB_|f==YMq|LG{5G0ef&n^s>eq=c^j{ zRK|;Al3^MIU_Oh~j{=CB!C4gJ-M7GYc3$1bBa!L;TucACcW98Hc#2S=j|3AVM4Ldq zoHX@8Vf$^tiWP|;v=?n`jbdfaEcTwFE13D>)*bx0-aI%WApC2NCu_GY_g)PZ}h8>tS`ZeUtc}Y>H<=_kE_BkB?VZf$X>Sn1c=xs|W zB;VK~vqx9r{+YxJ6u6EMw{g>^H@ww0K+V4hZ-gVp&(ax=G@i-2eldoazr}8DZd_T> zWiw*RiE0Z8lBUkNJTRlc&W`}3WqWp8nQ{C-bai!^fQfb-yh_<;m;hBSxbQmVfcW>X z*q8(|7zIlhS2Nw_!U^R0rzSR0hMScYtQt%Xl!Nbm6B70_9PmUJZ$faV3Eys3=GGq3 z?1?U8oGb*LnrGLS?b+XVVw&&$Ub5MVRc*8D?+h~ut5w+V2ZC;XL4OX+V}c(0>6lwF1Q2+)Apw020U|E5E*~Z^@cUxw;Yg0=?FuP7{0rF885!G(r zq}~k4>Uk7*W7&3+YVETC6fAAAP+N$*ixH9eA8mNUjNRq5Jzx~HTt(sAwYD7dpBptH z>hfD7`ZZuKVAo$~%F0HQscy(NGY#wUy1wDPij|dBy=Qgae(Bx_SH@D4a;wpyh;JwO z7fHW*S;a5hAiQgVnk7f%w}kLAj>6`ccy$38w<%ga98P6CYj6EN`}vFZ*+jFyj^|s% zwq6u3E-oz8;KnpsHd*IL@QI`SC%t_9zMq(}@#EXUvcT2HuY7MDz1`O(Qn;ogeTQ){ zW;uWObT65}LKR{{*;za>DCVs-2D$I&TiZ@yf5v`}nc`7wpX5~va(tZ+BKGo6L*u+Qr`&%PK^zS5LM zahIeS_g?tY@Hxx|=4^PeD)0a+QcruIq%;%BGaFF&I?)Bl}(IC&txr58!r^uB=COwd=58vee%i?YTeuL z*~xEgXlinKhqGfzpYW>yyr8Z>^MAjc`V8Sprt0R7-9y?eM35$QT2>o&62bNc0d2hK zg(gEc#`8G>?;fN^dZ1^v?dSR>;D8IiTmivBVh%~<|HrlRj3Hs$^2WxkKlr2zB=}z>Q5e|h(cR}NV`H+$qyj!Cx&3yw&6Gx~vjk*wLmlN88`GhdHc}NV zz2Nbo4dGTZ$E#mLmWdAU3lL;~_6II18~)uKOhfcFi}`>5oE^D@S%R8<1w1K_Y1{qQ zVnk<(C}pU71CLo^>ouX4>h91c6pABjY@>JM&S~ySR{P^XQhJokIfke^4KU`;@=f0Q zUTm1g1r0xQwHkhC^ckQun&d!j|9;#G*{B=9zf*~0wVCv0@Rb@N`oA8h2MmJ97Alp9 zzBt6;&hCV5nrTMsT$hcixi@NDJK_(GXZ{??$RF2o?h2)R^wE|e+%zEE*b4gQNp|Aw zAI0%s--UTRug|Sl1XAXXe+X2K)Njddz9d zwquj!{}dD)iGj5?C)FX6{StI%KNau#F*&@8JE) z${^WoUZZg>2PVz@`M~5|_>4Blo(|-xvBhIlk^m%UlG)MngA$?ACa;#Qay`Hp7>2LH z*2jqzq(1Op9v3tn`7u`F{s#3rE!+t33bKXSdA{4NCiDYR2V+F({S}Oj7u^#+FjVsR z%&w+`F9r>_^%rRH3-$MJuw2Nqo~9e*K|5gt4ED%bqp3a=H5LY7a1AZq+_RgLC5)e9LE7xh^v1U_xS{;;_? zpGNBz5Ec&k_ar1A;{iISVX)dVDgT^IQQT-%6g8DIaihRjLV`z3nQ~f+%=$YA<_LR@ zy?8gXNXL_fB&Wpd2oAls0U0~c&_n3*o^k)2197Igx%o{fe0lCyY%c&=`~Y+-7cZ4? zPOBH&3H`W}{Ds^am1o$JTTb(z^JCb{KBI5*A$2o#FR>pjmns@#_cyrRm0QBDepyQ6 z`p_fHSTcT=h>F!LWWzO*)9fWA?vYYUu=T!)%fbU?lGHMjE585qd?9)W5-L&m>Q(Sk3>Z494iZgq1HCCed5^jzU6$xuTr}{= zsFmg${6UmTUNj5(I-gOP6+JCNmAB+pO4V>t=eyAm18`!dmJpD|!C%z#mSKyk1FFH5oL?qEeOiE%8CmY|FrIN6i!9g}^YEtvc&8BE~%henIh!;clI zxYd4cVVcE1ne%=_!}SW{Z4O|ZE_zBxlG4=IzaE5+Z%N~DEv5pajw}>QiU4LT5{Z=( z^`K5C_jEeDnpH|!yFK+bUSt9`(}tC9%RDD(G1jW)ynbhb(QZ#`nFhMhrB;IP`=8Yg zN~P8eb_0iIlnH`+&-lI37uG?Z!Xfi!!kc`?>0`fji0hxs;S{>OJTN%A{C(GCB#1iP$UR|v_v_xVHiEL7=OHi$`+z=T0X6NvNY$*cnpv)i!b+kG zpIc$<8QM|wBB5&O2p?~ULV8bB6!W9+*~ZGT?8~PBd#v>?Z+rTXo#W&dsJ>tyD(SvDx~y;8 zi!I%|K5A-_CDa(ANT(g^26N1!jwcyqg>c{U`tSG2w2n-7YDELKdT}7dqnK@u36$OTJS%-n z-DSA<=R-1MC}kV(w_4Y|T~<=^vSh+grGaWZlg*3Nh(f2P5~$y~_1r`Z@B3nY=Lry) z1-!NowfpbGt2j+J1rawDul$vI%*|v+46y;CmyV9}^vWbzAFL}Q>VP2T!*t%j_|#yj z4y^P8Av0SF`27zunknZCKkr?)c7MipR=N}o@58h&(EN~^sKjTFUIY9E*kjrr9hM5Q9*HtJ^Xxm5*{k@%^;UflEXh0y{bnFDQb;PL)?bvNVI8< zY;G?4tDx6`;ga3XF@kF=;?7oCPzdWSffjEgzA9?|hkDy;G1G?m| zJp!^eOe_cUtzaG;CShMPUs`l7lII_rD>kx^lsNJ&QtoYDqkmqNL1p;C7enu4^nb&r z<`)U(0O+o)g)U;1UqackR+~xqR$Q(W6FQsS&|1ICdG9)Vc1(KB&%Lor*2z;?QETf$ z)4UkC_?rbXU5Vjj{ilv`v%X`#elP^dc|CLzeF2|r_}M%LyFHQN9=?OdmY26_e93(d zD2i==1>63}q4Ks=;GWMnyGlL3gBE*EGHlk((fpefel*tUlWJn+ybxt{A8?BJm_?%* zz08)+H}R4RNq_b4gLrM#H`Va9e)j!prU)ust*2{3GoKEr1EiU_G`*Zl&v0Y0Fks#N z8>-|Fsppc`MLOhT{`s+-#b7LorUdkxEwC0Z_MV*bxBAazdktlUAW2>$*J1ScY%SU> zk*yqf#V1@w3-NvGQ{ttPU){z59IH?FmW7-Hue6c4Q5&#XX;vmFy12{dd30Rd+5<+KCE6+D zw3oxT62Cp+y7nD;T3V^p$Xd+epEOzfJWu@96bi%dsVDO9LvDxY_=%N)y_Bb#WLr_# zA>v!B|BxI_#B8tI*rvIs&RgF?DqDX)^!lsxiKaghE2AJvMaz8o88k&g^&j$jr?o>$ z>O*QNCjGwMzCy<3OLnZZ4fczWi#OQs?1+I6N4>6}aadV`Y?G2J5PW^@rLi|H6&FGx zb{qwSi@vTW*0HkB2u8pWX+=2zm#tjv*^YaHc1TP-p0!|-<{4rhmp>zgfKgV4P&UF- zV8$gABcpF+GzJ0bkJ968g|j&n;x-e{j0w1N~48_CfyYI*SIQkDjwsD?MOOK-Wy-S>Y z6h8s|TU`u5^MvmPqz@ZkBv(%6i&QJNw7U8oL(BOVvt}Jh_@QVecw%`TD)*K2CLHwF zX^lOiUczk$VB$ieRW7n&VcJ2O3iG9k4!}Y7Hea6kzL=z_xP~4~L|W+4?1Bwc0U?s> zM!#NZVAu*4P7~%c?#ZXl5D$Zm>%AW_OxHRkxHHxpHYz@Bm zvizfMUe2a27=e}2rok>gap>$_5!yJs7D+a6(sCPgs!HxE$DO|t)K~;5!wboa9{*XN z3Ik#XK&^QdL4KY~2;)%Q@}&1vuH`)8Em|bCHC?UYh=g^@Zb{nC9Kk5C9Q|X>R(TiF zqk!lz+wda!IqFF#AM{ndOo5|`N12eg-*w6xN5iiuXAclz!I6`*-}c+rDZ{SUK#o42 znyMRdDO851xDW3LLn)n4A`PtBxA~0cHu-2jD}v^<0Li|7A)4`jQdlxRL-gfwF*wFK%pxA^e@BT-{^~6 zNlo>b5xZ*TrJi}gQbLhd%U)i>@9)L2g=_TR!P~Zc(n&si3Ibn2Y_xap4_V#_${J6J zD1}pv?g!ViPwmpc^M)}_6xqn@qFH@|?sleI1faxK#JKM^ecng>imH`Hkej~$>g(&f z4XfVS4nfmo#&$-eSyU`S^Z^Cpc+Q7{t?8#U6Y?VY{z`Y@$T^rk+6c?=j&oQpO;J;NE;}C>O0F zW#N{f@RYx8=%CNO+Dp1D4F!dLV}Xgoi7iLpyi8NKFJOzRYetPjq+zkz3%9*29-sit zK)fSEqm|?kWH029_XD5%1@VVhk=x3#YUVZrF-o*K8O-18YNa6g#O*srhx^`9OXo4_ z_up?nB`)e@wLAd35=wJz)v7W$1&g;ebm~x(XDga@e%{{R2KzfsKag2l>rh@>`}qqB z(-d+*adTl+r|Pp%FFw-;=08LC!2QD@7#8O|b*p4$KE^qpi@ImaC1O{X^>rVtQF-x> zSSoEbwf4cWg7h!eD&UCjQNg6mZ%8!MNEQoZ0tZms^8{~{rOC;(_J$ROunID=zrP;5 z9F$82va{3^WACB^1o_Z3!im(Tw^A`~j{`u;#70@;|Fi86J=-e$C#wM&^)xoMEB1d(83!kR}gs?=M#3t>8^{zf2 zclYxmW&TBt=bXDJaFb8v_bR0bwYjUyF07kX%Fnd%Mn}pP#h446Zkw*_V<1IYR4>4& z{T##pW&ZI$nEUKV>eDcE@lKJEN3&_cp`jESubZ_0!)hndHXhH6g->hp2%O%L%GB2} zeAv@8vZDRi%gHCN8}R0WvA?2uQt}ZEwohdXdbQJI+eP9>R*h!eWo%#y8}1q zp|F{jxbuO{@^c7i7N|}>jx}<&lxeX;`WaDH0!iy4aCn_tYqPP-ahqqDub*E#F`0`( zUhKZz%IW`nzF-^dg*%>B$4i|j{@va-AtAO?h=GN6h9PJbi6-lCTt1|vyIu8*nO4%~ ziqb~9R&xkJY3L~LHjtkzY382#gpxNT7-z+9_5qps1Fr-{L`2kftoHXS0@>L`H}3`b zhdkvtGwX2l-uiucd8ky&#Ghc9;tQ7!7Ks-htO-X zR!yKJO%mL|X_iYA^yTD@3FRC4L-Z*eq$P}iG zWBtO>{{GC&`X$_mbC#Jc5L(Q5{-ma6<{PpT3^iyR75o7RD24`^DxH3%SzFwc^+Qx$ zIBw%v?a@yVxnzow?bRQJLuoU<^)(~a?t@n#Iyc$4ZX$J_s$~86QY@pH&nIEkMa=S~ z3nBChoyMtT$P@knc_H!!SA_(;?b++lE2&7^ zdTk!bvv&7j(Uh}{l2X}RkHG`q5C&?)AVREs?rBraigdr^6LU>2Rp}6hHx4Ro2|d$; zWquGaZ>k|sLBD1aLMh3R-y2#G@i}qk5$Q5v{{@RhE+B}Dc%1O3%}CXe(>=R6bu7A% zWDvpN+84|axXN&7cQb<$o{n|7hWEg7RSF1MF+kf9ZXAIQ)`&Sdrdip#@1|ZUaU5ZA6 z{zm)>NUskPpM4|Td$+9A5k4A z<`3{IFemdn;Q3Cv%{KE}2vVbGbs%5Ujk<#oW0wvAgP$?*I8G(VD{?2RtIs~I3Q8gb zEag>zE1rh~=LD&Sl+O=91?I5E;MTibL0JzjSC!SirU?fx=IZvJ_M|gh0g!m|*~{3I zLz3Jt_smfIB+t$-+CExSrSk8OsV{1z~h*ceS$ zM0sz%BGnJGD9}i$v$_IA@2;3ddu5=!2eY&EGS&rR*cykWo=LAn+iy7-ybZU=h zrV78Pk$Hyr`6d1rFsaAQCrRr*YnN8x0=z zy!rdxg`C6lDTf0|xP4LQ>0T5wo36*OyjK5{G&z{wEi%GWZ@>fG@>}ItR5WAhL|E?p z@PT&{{23P`=_XHCCq~*?6aW1QC&G~cU;z6NN(BpAEyLpe`lJA69gtlILb+bxb2#yd z3DQ6$yYVjkcfxp6=4$-pQ@VjSHmX|wYHvhq)K>{eu@>ro<=mC-b7jMe`+EuC2vhcRB4DlN z3F-PX0_7iVl~RAY@*s($>$mohf-fAlWzs4u1pfpUUR`fy;jrIMYX_J6YKhExEMY`Ug ztOEC>;NY&qXrBc3+t|9WFripj((riYM_sQ@N7u;o8P=OK>M(&mt)$g`mwe{$x}hrHfd_f2r=z+WEl zref(?ly1Vrl(XC)+7HSk2VJpXLFkE>6Tf8KrOQVizS?XvFWX0a!{*H^hfqiapE~@4 zA~B?}fBVRFDC#-?{|Bh^Ts+Ka8k#!KnTce}w-rFTt^Jd%!MgEvzRsENu`CHlF*Pf9 z=+M;(k5z_)^5O;6FvY^H+dy>ze-u0~=Gh~XpW4(S^z;_VpOH1vX2DKdDBU{Km{t5L z3rg!lFtZFxy7v7F__>=4x-GzKZeqbF`x$tjzj0^vh7rYH@`weKELrC?Nithp8nq zS;Yn&u_&ON6Mk7*7xn=!ZC{FIx2P~E2nY~5y?aawLSMv$1*zGCaV*wLhus|ndaBR| z89|jEP~T87XF#AAS9G37l;|-$IcrDeM-+%^xZSA9$3}S#A75lr@q1yg7Qz4O8bQ6y#0~$;Z9#@B z%S=8Wf5>2ng*(_;FWNcW1k{(3|7XpN$-cZu&M`yC`ax6Mr5aW%+n@_BkaJc$M@ERD z98e)E5Ls>|o+l|QdyOz!$HsU}*utOFfhZ4=LtHMCc?DIX&5j*A#t8;ln)gyu4HMMO zix(|QdKUEICzfSuuBi3yNE?Z~ztQ2ojsLGSdiE=YQ@WXbfb46frf*lfr@(Bg8cyMT z82+Bo4w>^crwMnlHy3dHH_-SC20P=zg9b_GNXVrKM|8&m|GRd31nF$its)7e_uD5E zYt=XWx1U^xw4SqjXC@{PUpayr@u4 zL(yudL)qDFvr-#fPR`ou2*J)1L!;GMF)W_U^Y@Z?DVN#!9cNA+KD_B1Aw_`O+y~E? zy2m_za5}LaohlC1W`JozMekw z0Z)oAQY5oN9VY;PBiLlX3j312T{Hxwr=Rn!7&B-`!D9XGA5iPT&lj1$hB5LFwc{_5 zcMz92HIX6onLJ#5I*Q@B+W~89J%s=NkL8JPfCGdenBmhJDDViwhJAVY$o^#7NcdiU z+=jSRuo#dKfBoi^Nd$;o@rJgy4yD>*_>@t?=o1xLYOst{E?%bVIxTZqxOAR}7NgQoA)arH+~KLP5U$ecMs6jw=|0o8pk*{8qW^u|iYc+R z-#Ai?Q1eX!ljuherNPuWBo4XnqY;jjptA%@e{C9}a9Fr-A3fAmH(rpPqei_sV_MAU zgFQKHff-v>GB?BHZ&K>*5r?0|qCBOk6QbyTvTakBfjhMoHRf`svNp}ZEx6W8tX+>8 zteQ1*02(GjAIdqmUZDG-}FbzK$6W= zb{j=w#U0YJPqyZ7fyfKM47$(uT>6l{+br{V=SIQj=;5XEzW>JX*A#*_b0xT>t2~Dm zTOZFL{e?Zad0_U1l> zx(Zwk_jR7<(#gGUjOvOh%5!2gHxR-?I`ZmH)@l^xZRYFF5`|@4B=0s@gIqDplQY(N zk&LSeRYZYY?10I-bzAH3wZtw*%SoRs$A`q#HS2PFlKuaKfcG6<9ViKNI&E_nm=LiI zuqDk@yhA`5mQ=Z3aOr`cz}BaDg#L^eQ|VMji4!bwAIOxZq)%U6>len~o-j3RK*a!e zn5W?iA5+U-Sr7sSsNGY<@bB#Y_wQ_BRM8||nnFYRAAG+^dHt}FWrOATa{{*^-=?+m ze@6Mg2qLL77j%QMg6-?>a|-TC5GtegQoHUQ)HtH%J5KVKfLBKneME~`=&(}917q84oT>DLR z8jI~`!&66Tnp|MB1?`WMe)F^DO_$7CD8*1omx|6wXP5r@e10XG9$Ulz%0T@zW+EH@ zC(;;=?sAlzPnw&z#}xn0xIEY+*kB*K>TOS!{()3{`H+{K%cmsrJaHY-bwYLhj@`t2 z()=0*7xRwkD}js*5a=#x9VH^fnt7z7V=Go3j}v5XAe5wZ~5iO{XO5H-;0JRJ+b(u_XY`G<8=(;xr)ZOA(v1tw8D-UaY-KtjCkEoSfb9I(wICN_r^FSUx!qKRR zE4>wXijmYe(Qat(9Nrbo9XO_TXtpo9Of_ix`i!B3doFu72WTy~)FGM>9AQzb>WrUn z%kVOHfYkWjomC)!rIcDFlf2tT+!WG_8#vY8R~?4I+sIb?j*!>T+5G%lPhL&{Eqa7a z!%XC(m~=pnh9=h16~<4}XC8bMS9;3-6vOS%al6Svw%!mJm7>h<9a(NeaT;0D>ORzQ zGE!3AFKw2s%2W}bFW$28rjkS0zv<$Xqet|eOa|wD4p;4i23&upwSqO*jyTPG`vsY& zUm)O63fjprbBAhooTv``O|l4_KI`y%N?@Qn!Eru5TB`C1pCPBwZmN*=SY_qz-YEIg z#R`0R8{O7GH-_g>Mo^ZplW=q@N9SY}B1UsRYwR{bfUk`B?1Dz|IdBA)oD&t9$)>}I z*_?DwTx7J|t!l$~)~vAvunA>?4RPY#?^}t3#*aB;AT;AgRzW;1XvmUeNlK>i@r;1V zjliP25!cRzM=yr7uMu6JQt%%-?`^&28_j2EeSWB-PauZFq>~AxhWm#R;#c(7FAZBE#n@|EQ7uL}nyrxu*E?hM6T_$x^W%dV z+FPqpx%h4hU3*6q$~yD18y4lsI`X8)zb7)w{X3^y}R#Gvv!Ox;*Khfxtx$j=k8{!ZTPTtzJluX z0o=@3D6hW}WnzO2EL@rrK?*Xuxwl>>O<$=tFM-9IxJGg2ZbFKDW7@)FxFBdb_3n4Q z)>N-PE=TvNtLA$B{9x<839szSL)9_d*}{vQ(yQwEgY3~}RzF{R19VvoQb+QY36+dJ zfU%!6M$6-Jnums(w{b$d9 z+5qyh`xAfDsA{Ap>!)@fRKXfxCwQtb>8}S0aE%BUlfcGx1=Z|2$aL ztqXn`4s>_2TQkq0>kR&H*l1?EtA}Vqh-M@!(%e>yYK+JQ;sn%= z;-|?xQa7L#3Ws;Wt5?tdJ>YvjBd1&I09c8MTJ7`=c@qW5!ha!1motdlX3gQawVO1< zzEuVf6|Ne~RViULhAzcSr^Bvg$PCp&E4=9k%@1GHn_qOe?(N&16Dkb4)+!`)#bD-> zC-=CUMMZ&-1FnqpsigXW^yDK8cTqaQYe_)LC`jV88)^fUn}rA{&hK}amgO_C6zy{uxJ~M=$oNn)-a$xDCDv8m4q)~S0uuPM(1%W-Do4kSXpQz zMGrf4AQIi+SI}_`J9MDAGFIm6D)9}}$nixEJ(O1AqeXoxDFDyesd{?1rDbHUI`rPr c*%~>@Z_uQ7Ic->Wgn>T}vt8^i+Ia8!Kk5-IA^-pY literal 0 HcmV?d00001 diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 1150957..6dacf63 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -17,6 +17,8 @@ from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import Speed TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] TRAFFIC_MODE_T_FOLLOW = [.50, 1.] +TARGET_LAT_A = 1.9 # m/s^2 + class FrogPilotPlanner: def __init__(self, CP, params, params_memory): self.CP = CP @@ -36,6 +38,7 @@ class FrogPilotPlanner: self.slc_target = 0 self.stop_distance = 0 self.v_cruise = 0 + self.vtsc_target = 0 self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)] @@ -44,8 +47,8 @@ class FrogPilotPlanner: def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego): enabled = controlsState.enabled - # Use the stock deceleration profile to handle MTSC more precisely - v_cruise_changed = self.mtsc_target < v_cruise + # Use the stock deceleration profile to handle MTSC/VTSC more precisely + v_cruise_changed = (self.mtsc_target or self.vtsc_target) < v_cruise # Configure the deceleration profile if v_cruise_changed: @@ -152,7 +155,28 @@ class FrogPilotPlanner: else: self.slc_target = v_cruise - targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff] + # Pfeiferj's Vision Turn Controller + if self.vision_turn_controller and v_ego > CRUISING_SPEED and enabled: + # Set the curve sensitivity + orientation_rate = np.array(np.abs(modelData.orientationRate.z)) * self.curve_sensitivity + velocity = np.array(modelData.velocity.x) + + # Get the maximum lat accel from the model + max_pred_lat_acc = np.amax(orientation_rate * velocity) + + # Get the maximum curve based on the current velocity + max_curve = max_pred_lat_acc / (v_ego**2) + + # Set the target lateral acceleration + adjusted_target_lat_a = TARGET_LAT_A * self.turn_aggressiveness + + # Get the target velocity for the maximum curve + self.vtsc_target = (adjusted_target_lat_a / max_curve)**0.5 + self.vtsc_target = np.clip(self.vtsc_target, CRUISING_SPEED, v_cruise) + else: + self.vtsc_target = v_cruise + + targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target] filtered_targets = [target for target in targets if target > CRUISING_SPEED] return min(filtered_targets) if filtered_targets else v_cruise @@ -162,7 +186,7 @@ class FrogPilotPlanner: frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan - frogpilotPlan.adjustedCruise = float(self.mtsc_target * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) + frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor @@ -181,6 +205,8 @@ class FrogPilotPlanner: frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit + frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target) + pm.send('frogpilotPlan', frogpilot_plan_send) def update_frogpilot_params(self, params): @@ -229,3 +255,8 @@ class FrogPilotPlanner: if self.speed_limit_controller: self.speed_limit_confirmation = params.get_bool("SLCConfirmation") self.speed_limit_controller_override = params.get_int("SLCOverride") + + self.vision_turn_controller = params.get_bool("VisionTurnControl") + if self.vision_turn_controller: + self.curve_sensitivity = params.get_int("CurveSensitivity") / 100 + self.turn_aggressiveness = params.get_int("TurnAggressiveness") / 100 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 99a0dea..284f69d 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -85,6 +85,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"UseVienna", "Use Vienna Speed Limit Signs", "Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US).", ""}, {"TurnDesires", "Use Turn Desires", "Use turn desires for enhanced precision in turns below the minimum lane change speed.", "../assets/navigation/direction_continue_right.png"}, + + {"VisionTurnControl", "Vision Turn Speed Controller", "Slow down for detected road curvature for smoother curve handling.", "../frogpilot/assets/toggle_icons/icon_vtc.png"}, + {"DisableVTSCSmoothing", "Disable VTSC UI Smoothing", "Disables the smoothing for the requested speed in the onroad UI.", ""}, + {"CurveSensitivity", "Curve Detection Sensitivity", "Set curve detection sensitivity. Higher values prompt earlier responses, lower values lead to smoother but later reactions.", ""}, + {"TurnAggressiveness", "Turn Speed Aggressiveness", "Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns.", ""}, }; for (const auto &[param, title, desc, icon] : controlToggles) { @@ -451,6 +456,18 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil slcPriorityButton->setValue(initialPriorities.join(", ")); addItem(slcPriorityButton); + } else if (param == "VisionTurnControl") { + FrogPilotParamManageControl *visionTurnControlToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(visionTurnControlToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(visionTurnControlKeys.find(key.c_str()) != visionTurnControlKeys.end()); + } + }); + toggle = visionTurnControlToggle; + } else if (param == "CurveSensitivity" || param == "TurnAggressiveness") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 200, std::map(), this, false, "%"); + } else { toggle = new ParamControl(param, title, desc, icon, this); } diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 06464ff..19d9162 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -52,7 +52,7 @@ private: std::set speedLimitControllerControlsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"}; std::set speedLimitControllerQOLKeys = {"SLCConfirmation", "ForceMPHDashboard", "SetSpeedLimit"}; std::set speedLimitControllerVisualsKeys = {"ShowSLCOffset", "UseVienna"}; - std::set visionTurnControlKeys = {}; + std::set visionTurnControlKeys = {"DisableVTSCSmoothing", "CurveSensitivity", "TurnAggressiveness"}; std::map toggles; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 0dcbf7c..5f27f64 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -650,7 +650,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size); if (is_cruise_set && cruiseAdjustment) { float transition = qBound(0.0f, 4.0f * (cruiseAdjustment / setSpeed), 1.0f); - QColor min = whiteColor(75), max = greenColor(75); + QColor min = whiteColor(75); + QColor max = vtscControllingCurve ? redColor(75) : greenColor(75); p.setPen(QPen(QColor::fromRgbF( min.redF() + transition * (max.redF() - min.redF()), @@ -1320,8 +1321,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { conditionalSpeedLead = scene.conditional_speed_lead; conditionalStatus = scene.conditional_status; - bool disableSmoothing = scene.vtsc_controlling_curve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc; + bool disableSmoothing = vtscControllingCurve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc; cruiseAdjustment = disableSmoothing ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0); + vtscControllingCurve = scene.vtsc_controlling_curve; customColors = scene.custom_colors; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 4a6d9dd..a5e2dc3 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -233,6 +233,7 @@ private: bool turnSignalLeft; bool turnSignalRight; bool useViennaSLCSign; + bool vtscControllingCurve; float cruiseAdjustment; float distanceConversion; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 1b8b794..6ad5ab9 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -280,6 +280,7 @@ static void update_state(UIState *s) { scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit(); } scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); + scene.vtsc_controlling_curve = frogpilotPlan.getVtscControllingCurve(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -345,6 +346,7 @@ void ui_update_frogpilot_params(UIState *s) { scene.holiday_themes = custom_theme && params.getBool("HolidayThemes"); scene.disable_smoothing_mtsc = params.getBool("DisableMTSCSmoothing"); + scene.disable_smoothing_vtsc = params.getBool("DisableVTSCSmoothing"); scene.driver_camera = params.getBool("DriverCamera"); scene.experimental_mode_via_screen = params.getBool("ExperimentalModeViaScreen") && params.getBool("ExperimentalModeActivation"); scene.fahrenheit = params.getBool("Fahrenheit"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 07630e1..8302854 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -189,6 +189,7 @@ typedef struct UIScene { bool compass; bool conditional_experimental; bool disable_smoothing_mtsc; + bool disable_smoothing_vtsc; bool driver_camera; bool dynamic_path_width; bool enabled; @@ -237,6 +238,7 @@ typedef struct UIScene { bool unlimited_road_ui_length; bool use_si; bool use_vienna_slc_sign; + bool vtsc_controlling_curve; bool wheel_speed; float acceleration;