From c63cba4e36659b038f9561e598c23ccbdd5b9ce2 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 18 Aug 2023 23:36:19 -0700 Subject: [PATCH] Calculate average desired curvature using planned distance Added toggle to calculate average desired curvature using planned distance instead of vego. Credit goes to Pfeiferj! https://github.com/pfeiferj https://github.com/commaai/openpilot/pull/28118 --- cereal/car.capnp | 2 ++ cereal/log.capnp | 1 + common/params.cc | 2 ++ selfdrive/assets/offroad/icon_lateral_tune.png | Bin 0 -> 30135 bytes selfdrive/car/interfaces.py | 2 ++ selfdrive/controls/controlsd.py | 5 ++++- selfdrive/controls/lib/drive_helpers.py | 11 ++++++++--- selfdrive/controls/lib/lateral_planner.py | 6 +++++- .../lib/longitudinal_mpc_lib/long_mpc.py | 2 ++ selfdrive/controls/lib/longitudinal_planner.py | 4 ++++ selfdrive/ui/qt/offroad/frogpilot_settings.cc | 5 +++++ 11 files changed, 35 insertions(+), 5 deletions(-) create mode 100644 selfdrive/assets/offroad/icon_lateral_tune.png diff --git a/cereal/car.capnp b/cereal/car.capnp index 69daf1db980eee..3398fb70c0ec15 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -501,6 +501,8 @@ struct CarParams { wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds # FrogPilot CarParams + lateralTune @78 :Bool; + pfeiferjDesiredCurvatures @80 :Bool; struct SafetyConfig { safetyModel @0 :SafetyModel; diff --git a/cereal/log.capnp b/cereal/log.capnp index 99cd96614421a2..ba4454837a67b5 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -996,6 +996,7 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { accels @32 :List(Float32); speeds @33 :List(Float32); jerks @34 :List(Float32); + distances @37 :List(Float32); solverExecutionTime @35 :Float32; personality @36 :LongitudinalPersonality; diff --git a/common/params.cc b/common/params.cc index 98310657f87651..4b5c99ad9ca38a 100644 --- a/common/params.cc +++ b/common/params.cc @@ -91,6 +91,7 @@ std::unordered_map keys = { {"AssistNowToken", PERSISTENT}, {"AthenadPid", PERSISTENT}, {"AthenadUploadQueue", PERSISTENT}, + {"AverageDesiredCurvature", PERSISTENT}, {"BlindSpotPath", PERSISTENT}, {"CalibrationParams", PERSISTENT}, {"CameraDebugExpGain", CLEAR_ON_MANAGER_START}, @@ -171,6 +172,7 @@ std::unordered_map keys = { {"LastSystemShutdown", CLEAR_ON_MANAGER_START}, {"LastUpdateException", CLEAR_ON_MANAGER_START}, {"LastUpdateTime", PERSISTENT}, + {"LateralTuning", PERSISTENT}, {"LiveParameters", PERSISTENT}, {"LiveTorqueCarParams", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, diff --git a/selfdrive/assets/offroad/icon_lateral_tune.png b/selfdrive/assets/offroad/icon_lateral_tune.png new file mode 100644 index 0000000000000000000000000000000000000000..ba83e3a3b229149a0fbca78a43ca279e684639d5 GIT binary patch literal 30135 zcmXtAWmJ`G)22moi{ zLf>59G57Yv=hDQeccj9P4bZX@l!=CW73h1+Jbm{5ioWDY@FRUe-=iRB^m`+L@Y_$H z({!)jJ{xIXKTc`C$TnA89&aX;8?^+c>wkN_`(KKaAWwdLjPOmuWBTW$+JZYRf^BidXP{l~wmILlkRlF$j8 z$nU2Re%x!mY4WNMp?z*+dga`zfK4 zXh26L{6nPtp)#?#DfxPxB3Ckv$fgnkLtG`1L>zH9 zJR52GfIgw%Y`s5K@D?6=`i&BGRjnZ4w;5~^Nk5eBKbEuC8QNo zJJfotHDp08Mqlrg>2OEQ**#LAA0dh8aN_6U`tr1Ff^4DIk?z%GxpAKL4s8jA=)QwL z`t_=q7b&r&l~qBzUCjz<#D5=Y!|I;qIt6MmYn&xKu;qU3>gpFMj3t73}nPWadBs)bB3tWt6vZY(_*V8YsR4K zqP?mfGUmI(R%Iktxl zvHK>TdU?ds#@K8h{#CPMKe7qvDbWydAt%iZYd{bg#A{!d(n1;QZk-vt+_r1_2 zR$IdS0e3~kpPBjj`3GZTW9a-;ioB!|Yfn&@oaz{IhThwAQS5P$M+&hf>-MiX?K0q? zzUM+i%fZ76np<+vl8^rqi?otNwBP=KS<59K-!2#bNm7|q9HB=V(Ge$yjHGyg^;`3Y zMb*%WXXBt5XTci@<3KrNc#9)dV6FFXu?CZIwT}I5jKI57Q&X2jckd4P7m;8Q>PdUI zE`O&raLRm+i8+`U7bjj;Scp^ax~(w_=XfEpcTLGd{#DT2V(ugC)zu!1z_G5Ouz56IVws#Hp;lqcj zTFmc=#A7x50(yIUkCq~eMmApuhlH%3ZjIqa`nRVEy6wz-wXOP`n>+P4zdq+ho}xLc z&X=&c5?x+Wsc?gDuklALxfrQqPF=jcug)?vGc{zxTV30i91^!qE?&_mSee_{>{NU= zCUD3s;TwDx7$~nY%3L*d1$Vv0MvRlo&B#SgNU6srt>Sk!XHubM%QNEk8di%UY zgN#bZo!$BOY+bCb%JySRCVC!r!i(LSg+JsRCQ?pXT3VkJ^MX!Jvqv`no_UMx1u$vn z2<3BB)~A?N>qIexr{|7reuH~#Vq;}}Z77Y#@*UOBDU$^QQ^2en@iPRUGCeFh`kFvI zHuD{G_nOlbJ07~OM3P2Pe@|cEm0f$-9nosD;?R>mXsaUsJ$`KURgZNq()j)8nv)|f zf60HxvB-jNK9_%R;phmtu6Ez)*2BlJu6`32@Umf9Ocxtx4W6EXlAN4-uuRVF#_Ym$ zMYrFx{8Z;zkU0* zO{W4y(!yfB^V6qKV_X!wzD%9Bg(w!qa;WyJGxPB?ad2>!ef~_nYUW{BHX6#xv5Sk-bqG703G`6j(s;bDF91=Zxv)YsJx2?5x z9WF9&c+&-ukCMOg-1&%@tGG9r?M6*a?YJ%R@r!6^HJvkJxDMZ6QtjtvI=s5u6+_eY zA5jnP)7(S;Eafwd5aZ_|S|XO^#iga!PN%=;Zr;Yk)L`7YwY{>uOuZSDk&@DrUtCN~ zN+3HXw3wT8^z|DBnVIo~ z$Yb=G*x2@dt~os_9NYTMnPTHFM!Qe$M>*VWeIrQj9VKHbV{X323)Alw7Cyyl^!?j< zlGNW!SC23+C6sI528uC?WQe8>Y_P$J^!adiOF>1=)!2AxKEv6=AUpGufHb~DM?95A5Z7K%KS`B zmTMToe>AM&Vn=LkZ9VDe=nzFddgMo0uI=7iC)OOoH)t&$_x^qN<*%F=EWCT1tM{{% zCkeY8%kUf(^X{sxCqRwLVg*%nB(xzW3q>9L*BtmPhAqErSaD6$3Z(re>B|_|{BjR3 zSc!}M$V}#q-7I>`DC= zOh0>i&YSY{Eqf;`OtUIUzxZb?T+S7)jfImiEqngi;V+Smp=NWeo9(6#*~iBF3q}6< z5fVui%;mk8hkuOc@V!@g>sIp>v2>$C{AwOU_|pdC>=vWi)a2xvAY>1|u-6~v{q+>F zj86V?^Ip>b(#D(qa9q53s!~)vK#Ke)}k$j|`>x_D6Pm{y}DO$HP^5bv?aV#w=yN#I~jHNjXF7 zE&@z|#q>jUKO%Z{sJ&KQ{6RRbcF#~0gWWu_Hccb;?fZo25~zP?O>_8=-NBZI~D_A~i- z_`=>oGtI!7lPDQBhUBCTPjf+*4Ug{pboHwZn1sKWqhx|U#{PR@Dg<1N3|l%gRh5-8 zIKi#Fk`fXj6KbgTE=I|{Ctd)Ooa*#^%Rifbci6ma)9M`o9BhmeY;q^HpL$`Gi&&Z; zCa@oGibAh&tY?6C?C07)?l-?nWCHGB9MBL;-{`fKcB*zhjz>OY2-k^RIddBnb-@1X z*1dH&zUI^~g;=E{mKI-eZi^C~+^R2Zv4 zf!H21yKPVOembGTXHTD|t1u=#LTeGW^;JNWXb8&DCx|C;IKHskE-WmJgsbc)mi9^h z@WI;B*&6-sYjOgv#|-0ThHM#pj>|mIyJ9;`xOGlP%s5G)0jAkEov$x1_+IuC4I-z1 zQ@Ohf!>}EnF!cKyAd%%SR{N`yp?u1QAfd-)3);~@6gfwBj5AK#2ySDypn85??k5H^UbNpSx6@^m34I@-5Cg4@d3$?0=v zM~Ap$7K&II8J-u_H+T_|GAqsu;Z1E8RS6VD&8R1WB^s}Cr);kI`1st{R=T_Yd|kgh z-E01_axkCpG}o9sgggIsJaQ98M_>8Ou3^4fVcM?keN@lsTem1khy~K@D&&wCd1S9| z7qu8^nbgeq;Sdn~w4!Q-<>DnzW3jEkk3NA50DuNF~mfo852|j<^ zomt8+Lim)+GRZm3u+tM0KV9s%5t2Pa#@9Ws7+7-9D=IFgDo|!%6nL+X_Dn&cT_%Pa zpGH4lBif0NidTu56xZEeuDeOdeV;ZiDGAfsBUHIx*h^VQhY_y)7J&YN&wrH5Cj5l` zuCEBc+RYV~6zt58(giHg&=HI>F))PVaFXL0mQU>rxd=24SvEu4J9ggq>615R-R!qp zJt08Ehm**OMx( zt3OL-U6YZP79MH#yCIX5l+3W)(bHvqXOgB^Lnb}c9Q55Uaz5a@YU838gc9k42#6p3_VW#Ojlohu#xL~L)lc4PF^8ZjAV!9b&Lx29RW&qt1NIoD zQxWWlIHLKr{NJsuyTrdzp1j05-kz`*49|WLHT8&!Ae2NrwwaSWQr+O^Mva}7l~u8( zp^rLka_^>RW4hzwcU0g9TX*l=;b3ND#L@d^eEptkc*EJ*d4PGH1%~l9IR!;K3^Y6&{2+zV@AlpnoVwHneB$ zF!?SK5&a_HPunm}#k?18zXr3`UJj(sU6=X&-To=L{#clWrDWkBItcGrrbZ!=jYsOy zh)FIQk#|ZUJ`It_z8h9jAr?ox!52s1TGBFvtFk6P%NyA&E!D2Fc%h>+eIA;oRN;QG zoc};FM1dMAZNY)cjV!dZtWJ-01z@bNa?#jA6czuW#4`^f_uoS+-)S(UrKK^yPy6(3 z97)6JIXlT>czXHwbV12`DVS_g+VbeDaa+hF(3jI^a&pQPcpw)vCoc6_|+5hc-+k1%6>F~liW2`BA< zcZFog?9;`PG!V2PX|1%_}6x%e*@FJJm) zynkOCAd?etXL8T>^3Sdiv2>pM3Mve%3r-=SD;h3}5`kFu_0y6~#NAOG9J%XCof5YH zkjOr4pwUC*Cpsms+q&cEMoJBv38vo|n&QG=uBCv9K_6!dS(ZQx$kYs7w7Dw+ zMTC5%N~XnTl8?s%gxTHSKSvJlvf<>};0qO%p$G+f=L$7ObqNP;$!(-O=Pl`>EbKrr zmRK>bK7P10-}ANPPx1-$_oWb0LkK18K=xVaX?fw{;r1XSExxet>QChryoHGwl{sYj zz+1!*BeZSvlJ9p^k6FFGzP^vz!?^r0>yxba@83UbT5TH;y_w}^W&K^(-rj!B7tO12 zReKB|P*OIM+;JfJ=vL>}d#ZhGoSt_qx-ue{J5@{$RQ7{Dz}kNB*v+1`7L_%5{vgZh zOG!z=D;i^T;G^mb3k&nmV?A}vQpWaYit(Y7kJr%FuCst4S(NU+eA@@kj4O8F*(d)a z&=fBE;Ixv37~^AQ-N8`#3iPxNRDjTn^6@TTNor@TTFj0B^jjglYloUJ20Di~k2^=b z^ZB?Ha#hcfk}e<~7I1KIut}^UqgNQ?kq`I^A4f)ZS@bf5SNc1QZ=ak3CTLQSlbhU& z=+M;El)W7+fj~*evG*e%-*llE9vP`Id->8nUC_<0o^2gJa>$W?(f2VoML&~^z+W1P zEam8IOVdeA2mAYhv~sVUC~5((@A8|phl#)`zMb5g^pNPSgdi*KU!}T!iz-Me{R>@cBbuBe&||4Jn3y!;L;(P{4ZMbT z;~$rN#>nCur}qV?dcjmQ<_({@j3?@=NY8Id^kDAX#u< ziK>N)6Mo{b`2FRkOZ|^c7tUWU0%gJ#cU6D9S|+Du4-d}H&lf2ZdokAbw~@Y^sfE4I zu4d+$Vupy%{@KBbF7R@vT>c}83;v)Ma$c+9PaaE4OKqv{f6&GL*!?^UXo{h}H5=G! zUG3XvQZa9D;{kuXM6S3(F1?GtRq=+3+|>V{<{pVKw3{A(xPF@ zbL2aIFGjwcxuy26bEWq4zE@bkM;f92c7+dgYH?-%3dFuOTyR=cHUnjRFHjS5afCjQ zO6xiK8CA)v-uzWep9UQ;p!c9FBvmqDty*k{UD0_LafsG}zi%lg4sJ+>t?>)y#Yild* zWI@(At3k*+&e2VmKvyEfw8<t1s3j-EUo*R^-O&to(B1gP}YljxP$KiA#dAnY2dt0&#PcaQ%t zPFK%J+`t*7LZ>9yML<8yc#_U-5FZQOzr*I{po`|_b_JHmF!DOIj!Cmr;qOSs^_S?b z72$Y_l(waO7_KDH%ndTl%an97O+(jF08<`k3gj?cbQcUj-5N^E$oPT|d!ybzifsdmJ$3jmIQ7Dc!9LLj2wAUv)cGX?()UQE@Rc <#2EUX(Lbf7H*`A#_awv`OUA>B;na8*jVckmPK%->He9P zbTTy)RzjLFvg^~Rk@VQD1{xZ#@fj=KHiyl;bmc)1;@E znM_!0BR#s+x@&7|512_`;e!(L(}q7~&NNoYzX|Co~&rxAEu)@k2lAomXsX)P~-1Z=7R}r$1$ZA?c8^l8y^8av^ zpZ|lfv})9(LSfaZ?%wtMo~)X(m{>Zlc&tt>v)>U1yYJ-+$}*@V%EW6zg%wvw`ROk@ zyN`of_B{X{8c(A6nZ>o%(kI9(#;;P4MqncRqaMmuH|HpeN@Ixg`G?|Rt;A_5$u4<$ z<=ow8_zfUzD(Ay}Y?2xqp41d%PY!v=UvR8mf9W(BdLIjhAr4GYVR$rwEk;5_b$66y zK;hM>n%a?N;rZ0q7_s|-8VMq4rW#`uj7pW=52`eXmsL}ot^#v`+1c3&Z>0i(iFAIk znc_sjDaAL1ZuiL#aQR1QtCh8StX28>*lrR&s90^E)EJ9UBPt)LGLuHc%jE1DS4|0f zi`ji6NBe;`Y4cSgPHwbWA3;pJoaKO0=Y4)i78e(X{vVNiMyQ1Nj3Q3R3}Zxxs;Z8T zr!CADL6N`5Bd-8tH^}rZK2PEN?@#rF0cFa^k3v%P3I7dSa@7x6GLLP#bSV@6t$CVT zXdPyTU7O9Yg&j!iX#0)%rvLNErX4XUsfEkl{9oytej@U^MrV|BP?YjSPexx1guHnp zG&cYCT@q|Ry6BHDqk67+34@L@-ERAwmV z4XhlvZ^>X_S`s5#|8}EHg4IPeUt_;8uW8_s`+WGsr>-lY!n+FNGHo}Ayd^qz)_YBn>oySw|> zXw}OBN$fx_t*aVtA>V!Ncw`!}bU^Hr9EPnXBGl%`;S!I-(bzD_x(+lvn4lYcZMZt@ z-HxUfp0?nB(zH5c%K46#hezZ4%7MeTIYkl@5^V+gg!$xZE!wUiJ9cuB^`rB>FChhh zKFDx!aZlGbHZmf^!t%r4zO9(veLa1Cv~dq4uwvB$Z;=NNvNZdt?&C}wfKu}taF#!^ z#|;#7jB?Kt?xi0|OrgyeTSc#axzsmsH`pvs>CCHZXv_muntlxsWk)kO^5ofYgTrsp8?dHi;IgZtCmm4yu!iT6~1NkMBC7N;S`(k z|FZxGn7WKfhfYpTs}xc6oQ#Z&@!1SKfwYMRWo&i8oRa3gxh(`s_|Fs0IVhdpb`nUV zS>pT6Vy3q%KDK=#?IIwY)?%dB0OsCxmSm`1zH;mNaMAtZra% z%_OZdmCRE~lgOLH0)ukc(e%b*-IIloKAy?Qd^`ID?w16*gv#-@?wEh?K_)G0xX0V(IDb z<~#1^UD)aF>FGJdoJxP#fmp|*{W8xpY*9r*Modh_z|1_$ZavCiACyuKprZLParo)9 zE=ffvX{koh>E29@Jtz)ch8}Aq&wPCPEsoHLrA5s+Q;7Ubc)@CAWoEuYGBR!%&fATH zoHCa4YNC|wVSJ{b_A+Z)Qc6)~_; z9(^LDqPp%{>0?!Gj04fnSm{V3d16Ktv~J$h+EM zsZ|}Mw4qT1>WVuH^YHMniJ{NQ7`{YSxoAX(^=W`SLFkI10{xLaK0DR+P)_L8>4KjU zE&+k>6j`e30y+5HYAPy@CMR2CN1{MZmo55UeB;F>Wybb6opU35nmddMQrLvn7N30l z8-p@UL8|B&2?>dDZqp_)%{@MpLKLZQhb`*}GP1lu>*}nd_5NomLLN@difIA`(8l>t zfBo`{n9U{+@krV&(MdLMB=NwuF9!+#aMCxPGh#6N0B5{Or&9OdSn@Ev;BNW}n5}(ME z|NdLU43>zSR!!$R(bLwR+JyIOXH=mjs`Q>cYkphlh3WS_aCh$iTynT(#2@Ct2-=gtGseY|HCzQwzF7SDK``f#` zK`NiJR33*j!sn#G+s~NaO0nYV+jK!|X#Dos+_~gHFZ4kEP?S8Rac#FM>ZJGQPsOOWZ_n?E$Cig&fP~i% z`i!C9@45|IQToM${q%!|^78WcHS?ZKpEb)29C+Rfq!CPPpP0nS5hIClg3i5t^W~7L za=EGna!9nOmY~4B*S4lw+ng8N6!?x%91)L{()A9&&#^m7KMwI44=;xP(}_>Pg>5uT zAtZz$P7DH*%f8oMWAegq&-VIL5Cud#mK^4jde<5`@Lut9khl{ zcK_c+H7dmF_3$cHl!Q;zES{ZSaJWZ5%NfFevrOFgfg88fX}GViPYx?64)1w^TB|IX zt3Vo3KAt&CxjWzs3RPDMdB{+Tf<>=d2t9exkmX1CWQEhZsOlZgD4!M>?wz2a`vxPfF@Y z>)vvI5802u5&!ZQ|GJ+!9ELj&;}{--kXZZv|ni62g_0W<-@qK?ag6^s)F5Yql znUpCig1jQO&f^Yph^0{`!wXq$4aNiUZB%e$BG9={$RACJI%(yew} z>0Hneq!uzfjmEf1HB?nSR0@_jkTDI?Eg2`NsHpH`Vqpp1S1y~&j*66L194ccK<#>r zEW&z?{LRQ_pSa3#24D1z@{{qKNepmYy$;|E>ac{<2FQNXl*DDb!hE3RIGmo&cd;i?6-IV0Vj@qcI;q@5IH1HX<^}0fK_vF z4XZYy3q1z*05I)aCPBpXeoT>(UX!9Z9j0fBMA5uUxr`VDT1CmMm<5PAjJE~VV@<*Uh(zE7 z<3cfp@T?oy^rvEKHo#%m_5YESn7DfPbQ})B1J#$}pu9Uf>{>>OB==TBweWgk=~Oj^ zo-se-;^KP9%*@Qlz<{k6%S;{91rT(2aBy%E+=&0Wy1I%W+K4Ny;-Hkx?I-6oPD{O- zr&EGux{kT;39%|NL7$dAQ7wHYCMH9;J{q(DT0H3?&%azesrYDRiXeYHhEZL5jQIcv zTLAt?A}Xq@+ZdTbkwS=gB->q#igztWHi6g*2!vgA5&hvYSeyk;bywza%gixdD_=(k z=CGTECdi)I+5HAmK6l{?@JpoP&TNy9E@N`9A9!Er(1{wwei(h=CC3{hmS%Y;hcrAl zF{C*pcZd$G)3eLN#3gls`ig=i!5xUkInmJ~DPFRZFWQ=d8h;lX1JZ3X%V)u%4dc_B z!hvWF)Jz)1`!SjSyzvYWX;N}R3Px4^9 z9=HnJuYw{2D%1v`Rh{T9QVU6R@T%dCU6A?n{G3a_8(x&k6|Sl&h2EYX4hRcMLZ%1- z*4?XR$khCS@YP53vMw-~cXdrI^#9B@cvQK8o=Z&;HEy&I$xz6g1uhg77LNbf9O=T> znVIGRCsW4S+IkO6zNY(BR6+&SD;@`HA%{WmL%ahN^VrO)rjmt}-CQv{K>82G4sI2N zZX~osNPb;^Kn6Ww20HKGvb&EIXAj9$bbCW1BO_HF###D8QCd=UP2oq$#(%mP&VBX* z{wz(e;m*8xC2#-B!~KCf6c6doR9>aoGDFdWBqmKF?7&}BkkGJ$IIw!dL&4Pk-}BvB*wHEK+0=DZRVzP1 z<-XEsta8O-U|?{UL=v$q9i4ydjK}<_xRC}Qdk0`R+riqE2zf*WThhv(CsGlRhy~C@O75|?$pKFMTsfXHUadZnN+{4f| zD|S!9FiwgnN|Ce0pfBdV?~=F^wWnr-oFG(qP4srPe1DY)5+PD^@b{;qU{@pzn0apmMET}?K?mXpX`T>t^ zbAi<$d~dMt*;1&Lx5!Oyr+{GEPp6`>3viL`56-VF%s=x;ze#TP;$(0PmO%ATPSj(S zrDcO=-U&&P{0&F|(zy$zYn4s(epJkZ^e<*i*9!JHRD2|kz|c-; z#CN1L>KlKTocz+MZZ;L3OXjA!xrK#>9U#{$Kzl|tXfcU1pa02ixBpRq2_fnMpZ`kh zJU?QJFCFoP-{QFoai~IYFb*@h6P_;-b#@E z*9?Po8GyFDjZHBFCujVlIJsX$Z!r#$t1>wil%I6}MY38pcDsY#ln%&T?ebA~KI zu$TEZX8?}eZj&O|W{*Hh*>-se3W`vDmpZ-w;9`7?4_ABqi?Bml#<^+^2m(I< z2>N_@e#HxwbBnR5L;N`ymS3i9E9Pd%O4@bI!wY0!;}luvOwl?qK;l9Xrq!8$bik@BQ)PDMYF!Bcl2E`s&;s9`3>4?Xu>1>*~7F2LK)$Ag+`Lp6ds@Dp+S=P3TFq2W?2LKuwpL90kJA z%b*1vK+*p9;a-@n;}{Wk`|%@<~z_i7(fGBb}7 z`EI$?H~aI1t+>Ak5w?XmWTc4C#mD0So?j10Ozuj&SFku-?ubAQ%LNdGwX1-4fQyvR zhLOMf2E`tu_5_^hE#|Lp@NNGn$jjf>R##sGFP){YzaNu41Q$DSR?u}Tk(7wx9v)WK z$fgaPY62)|$U%gD@z?osK}rl%08ByI}_+M2ZjO%kuNx)oBq85_u!SJh){TA#-+Y zhQMBC{-vzXr(cx~&4Ycamh?3D)5zvBc-V+IIW92<;@8$oGTf&XIv;?NuoR8eYcLS| z;#-7|AO1KJl2&;SS*(DzrM@oqVmJJNlfxg@)kGFm=f^-m($j>!c!~h? zCvuo}9#_unHo(C!hT4)+R#sL}P{6|w=6HoRX^j~u7?u^h$jtKJDA@qaDml^-NHdwe zmCF7G%HkR}soc6P?^fN5nNOe{?TQ6&We z0)nP#wT1O-0~M8n5UGdfj{<28ydBu-Pf`>H(U_v| z3%)*uX_gFAzKoDYq=9G$4Vr`NufYc!grZj{=8Q`N3nd!L%!W-@qA-|BP9#hJQC02c zTY6(v0@!-zRMJDdOEsM+I{sPw{QS3|Mz`R;&IRgY3{+J)`M9`_AEDena3cGjX@-<1 zihSy@huN~xdE1>l3fY5`rg&PcK#$HK?xs;#_D^FD4eMCOgjj>ABbD2&h0zh~YTS5a z@w@OaZZ7*>P!_@pj5L-|1RbZRVz zve{y&_*EzMvOoNa0jaPKI}oMmrR~xz)|+I_iMuo-nu4=IT;i5I(kcs{PV(OTd$8@H zW(bLvo8ZvU0kB$(URgP~vm1fK)CRj4+Ti`fQ7jQo_>)STLKqe_{WA zhQQ%T?a!Uq-UYypgZdnCVhLLm(>GteNh6Zzh~9ER{3kvs$y~ta;>&Lc9ic3vA4rrC z6*tYgv0MCmZO$Q0L!H~)4N{z3@!#9o_O!{tQ; z%j3s&PVlJr08sdPU+&iEI^UGIG3E@NQKc!~2!E!^3uub(e?Pc8vv z2bytC9Rdg3oPXzSX|3RQWuUEnn>yVov@Hv6IZ3DFt4`U3W)6e(;HHabwT}M3Tu7y4 zC;{py9SSBlaq}YKp$->)t*mSm1-IGmbTuWQv>9QXz-7Src4b{{2;wjB-H(A!ix3hJ zgvpDbbNrVUW;w>MoY+}NhSQbugBQd5_-fIe>*6cmUA)LouT7|u~h zYiUhJehu_`6&9M=A^y4wvh@LQl>9vlt#39SADTMu>HoF=c23A%Bio186LGXJyGHX}g1<9yH z$oyu6hK7n>E)?&6wWXz+z2nL7v?xEn+}zBJI2|JD4@Vb}*x9gWr1eSF>P}G0px9S) z505%Ji006h3#W&X>VKPlo$L3$f0sH9u>3C)Ai+TE>?kqag+c$p;0aH%HP6YpNVaVR^et`}$tj{acBcpH5>jI=hi#Yl45JiCpcsv7vs5rp0#X z3%Z~Cm{I}vv+Y^j2lQ`;wk0b5&zX67Cw$j_B+WuX`K2;((oZu^ zdxJWsTpO_8Jpnj&Ier9r-J#>K;9xQ@G|xP!u8~?v3|5Ul~}7D}Vo=*QPb{bkANNmij&ofR%cvyM3JP9o zQ*~_=sukKH23NM_DJTge-vA7vmXF{5q`GN=A-)*ByKGBc= zx-)qJ%KLzvf`ZT%?l-A9{#O_s!{9MmT@N#$cxBbse;BpoB7wk^E|O@iK#dVU#Q1Y- zt3(^5Nyb76X?}uzRtLK&g-@S7E27+-eAkuvd_$EnX$KV@E7A};Gb4bFuPSFGdNp-L z+tbxdZh&I_G#QfyVAY|s&}^bH$T4pJ)0E$7d=x#$HLoBL9Hp};RT{J38Nwgwuv&32|Xd)+47@@a}a7LAtST* z+RRNB5%Bs`Jb>>460QdH0i0557Ehk-;9>Q|Ux ze#ah-%Hcb`+-okgtnx3CAw|8e85DH;-9SP~d5M*%YyZi9H!IlkE68OwNpi>;5WjsF z(1}OTEXGd59%bPT$HqWHgGc)0b)8;~BNfdXTDf4hMhb2;63B>qyMW<13JIzKWyT}} z>3oTcLm=#$kfB(^rHIm4CB%OT7$h8)Iekmj7&A^~a{O(y&(L)k!kyxfR)Ejg!TD2z z0ceeq2rv&YQ0l$F)MHPdEUAZeu+zr`0ijpZUu`Lo&$+eOr)6IjG~%HCVuRL$`V6VAQYrrF>DgRB++uQP14oC`so zkv`fZ9n}noGCrP&JE5l-!f~=<*L+$U}^DR^rAFW?y9waqHK zFH-k+ch_P&68FAiIr96H&%1p~^f$;5bN1i;QTM{6vaj*5a#~tzT+w48z;aJ|Xaw|L zJ@kkLYrm#Y1F?_v*e1RZVt0n6qUT@2ft}8P$1LbTWjJ5`O4IP3jj_B!axZt=ri+NA z%Ba|YW=_FjurjZD*+hn*<8u2j)P2U7b#+N8=lGz9RW8c6IhQx5sL?RYzI@ZWN=5*S9J3%Sq8wo~n!Zi{i2Skp3)<8@xa^xuG8 z+*E8Fx&K%nKc2!#>hM-+rDb7>QrFk-{~+q;+YE7l=M3SV_`h&XDfn)M%0?dj|0&@0DLn)~@u=6VMQ&>80 zBvqX?lm2lZe(7a-BekE=HH=`?Ry+xJvhKA*k@@^i?x61zITZe$r#n+eZIt#6sDmyg z)_|!B<;sX6I_OXZO4Nfq&LNaDuA!+()9!f121-E*kC`t-X6RsV@Lc;}@&SI4!077! z)3erRg&IX{;Gl8?FFiX*(lj`bFP1X3K`HA3XIV5REbKjwCLy~VH7LZBFk8RE%l){! z(A>=P+$_5vY)9t#T%T`TL7wjo$Y-MfFHV0!qRq_R-Ca&i zE%AMinNwyT-6(N@z{CEt`RjoTAxGjxYW*sU=c%in?rn?Hlm07RF`*2^;X^=~{FaZ- zeYR;ucoUsfE*GL?aumxZ3}Hb@bJY689_}M{WFh-C?9Ix<3?Oji`)I&Ik%>vzqW;~w z+5mE%?wk)FN=&GlT7U*dQ=oRC2FZTjw^GBPw)cFX&3hM$(E2p7%?8Za{@Jr&Y694M0Bi6sc+uG?PE~{L)zF5FxM$8(P`*IB=OL6`K_g8>LMe->&kf`X1}>1 z_jWcos4ORljpj`Pzd!+u6gSkOq@orC%|iY^xUeEi5Z_~lSQ#2uHywk6^#Z1B-TM9$ zHnte3QKX39b%POTmG(c(Dm~ISQY)q1F9Md}$sfABe*H%YLet;=1vw^W%FtS$xJ7X| z->dy0t~|N>y7$lSj9ZCev+o+r6jZzKdpggZ&vrwb99&UKl;(lY#RARk4#lUW$H&FJ z3*LOwlugKwjUmvF1Ox;qZ19c+CaFa3BGO$!V&G@ft6ls_ZL37lL35_4LJT>WG|9xL zYmxBk2l5@$JkxsQRT6ckHO zSQ3MM!lEkp$C?vtghv{2ru=|ReL1v*#l=M4{?u%MpP_I`3)$?;)_H6@vjXO^?D_%ru1Ojy_N}r1gC;~4GAh;--hU?F$Qrd zq`}Rcr3jW)2BdE~0MJ>3TEGRHT}{vgLEMhH1o*~C@grZH7yJZ-NJa+nojZ3*R12iM z_n$lQ3Gt+|n6ZIl<+Y=d2&<@ zx1Y~6w2USIYsXCnWrqKRL|gP*1TO3;3ZX4VnUDbb1*N438Hg94%ve+>n^a1JUtL5FQ{ER;$IF-nwzL@nh{!1{aIh@y#)ayMOCp@E2KfeNf^Q z*L5;$2HX?|{-GZ=3CT&Mtg_9_*WC%&$@g7@FN!O>5J#cCVG{d^3u1#f9rh-C@eIV= zZ{2|1V21&L1BsOvFFcA#k6zI3Q@E7;$80zLg!=RsG~K_NpK7&vXeo+Z{Y$^v7;n`nc7FXRI=ZU8AJ1%205W-&~C$6VFNvIS~^ zQR45DlgYYP4$`Uk|F5R+4yXG4|35Y-a&SmCQ3nlsg-~WHM=E6`gp5KhlCF!*YIaet!P=8k1HkNuQ*k7Q%VvRh;Ynr4rU50%N&*n|o@FSf`yVjLZuQ z=YE45)Pkhz@(Ez{1Jkkd8o&D8R06L2n{V5JmHkMDH>wgV_51WFH^UKh`G2*QlzfZY zbZVX2Vvl3qT$rNQ^wPWA+`^;HELa*^30L0(*Umw`EW|)_E(5A4ZDzH`z72*{3f~I^ z>}=J|=3cy(!Ds&dbga084}d$sf5qwNJtsQ26t?7Hih-v@6a>xIvI(HKf=v!Gj1ha z6fqQLCoYE>C}p_6$nE`XDhx0S`h-v`Y2IK3hM2|`wuHEC zF;^DY#BU1mJ&vfa*EYtm-HHvr)^$yM`2$(vi%@^G6<7cTbPi z?t7Sj`MG|?UR#%9wV<|!tW zWh%o!c1=yqC4l_xo@#aQr_=$Q6p?#Zm+!}r&6Lz2A;Xc%q_cvKXIji6$?IwM+(qnKb53nN3@ly9Rr(iH>- zHV{@xG_Hyb8)~YAS-&i8Prc5VdB`L!)a(=9_E)$(jsr5$%+AcTO^lDX2>w{T0%c+) zP&w;Z6vgH8vG0!iGE`1YB0h?Z{fHPv+m*}3E5Y8Y3W*LQ{8La+;t|YA+6OyG^+Hmr?bd zc+M!Smtz{Iu~aN{zI}ry;Z`Nox-mOGow{D8XTd3^EtuHk&e8%O-#_K&^b#moEXFl{ z?GY@hKSnNo*!LOIWa;d#)HL(W>D_Le;7oJnZpf)=Y99T{<$l=aLAmPLpm&5$XC(&R zh*1ZHNk_pH(D!Bl;J9$#_R&+Q0)en=!YNKje-dm{$kK23H1PKZL{Xi-UAj(Hjdo#U z(}6ES~=GRObQqFo%xJ!|G%1=m3sq?Yb(aLWH$6a|C|0N`u@n3}nS- zw;crp1g=T3_w3Tt{QUy^^*N4p>pZH&x1YJhB>9i@iK(9*W?@1dqdw{MU&h#v4v~*B zC#cbz3S&qmS(Zg)Mq}-c?}7&!Ypf?JhdTE=@Bo8^y7xZg>m8n4;@VwO05gT(f>Yd+ zYf-pWUOq{WsF~gM-wOwYA(i?2x35F9tcFa07RNH%Llm;bt49(DJA0R===8ekT*t2; zwp=dLSFPmut4|;9}=ZSQwJ4Lm%$83OO_@RyQd}5!C&-l_uWCtWP zIXXi4uDE`MAO6&!XWF_X63EqM@VFeh{tcZ#msW2o* zQ(K8{MW8`*%(QwBsPdsF2#_EG7N?<{N~XS&fnMpQZc`u64{7W8?cLN8vW1p}y84EO zpr6Q=VKAifLww`qlyvw$i@rt332PpQ<~w)q3L`v?yPv)NGfOpdcFLKcuy*wOoWH-` zFHw7UPlq`8d?Ok)`&GJxbFoE#rZ9wYXW+6z3G}6qXN8Js`i4Pa%O{}(Zh3v8+3ltz~uX= zs7S8R?yG`O4D@!4#pcbKnr8`XN#=X^{v5mdhlP!7MvMjc^*lq>_c9K-tALb8cK|tZAb*q^8lKaQ+K#GGW z1P^M^X{10J|KGb9VRa>>rr48!r`PDloAGu`N*_^wg?~S9NZ~c~#kufA1auAXNXkmG7?Pm)TWN*J0^X~ppC%Z5I z{r(f`^OAU|bnSS+5yrmBiidnI>>SD}LeuO5JPC4RL6J%naxK$!aDcpqDpvh*Z`aHFykF**VmU(sC5xZB) zSN=LLW$Fah-MxF4$GQrE3r9O06x@MBNmp3P%Z8QMUvPGoK96B8;QROQ;e+`v=k${| z`AO~Rn{Is9aI(z03njQ4-=Iw4(POipJ}o%_#J!BIE4-TdHK?UOxThuV81lx2X*)E) zV&4tVEeq`RhBGrW>z;_=IqScEb)WNp0~2NIfrcB}B*U}q>w^LA4I|4eB+pvyu=BP} zgQD6|wTN$$ERE`PS<<=0c%ki`zf^)>c}>mlRd82*10y3`Es6GKS}KV(&hHO@7K3i% zG1%AY6RjPM85tQgbbeu&WukAAPH%LIx|XWSIFT5lSG=={(?5M%O!N2{D}EriPL>Jd z4Y#lp%ZiRUcI+581bus5UAMd!um;obwQ=J{(!jX`gITmOF)j{{`F;EM@8=a(Zk(Z_ z1!A#}gAzniIJ{&3Mj^jF>|{7<28M^*FvI!uS<4bPC&>~7eAyjg5PIbED-(P24Rl2oDMXJ8klXJE^D2VVM#2+eUihQ!ftvp6fC$14F zL^ievYo+vhvv&u$+1&ubXlsjcN4o~9s;cUrIa87Ykl{8iSLKr@E9+BVU4HJYlpWH^ z5SU`VxxD}Rb7tF~F5MAPQZ+d&{jHD8TJUl#hf|C*lY_E5m- zodV!A8pgr5{3zDOLV;ua;e!X9EC2qvK`S{wVar`OLxp?mc6GI1pmUs@;Ab~YODPgR z|H)@?m|eD#Q?)u~*xlDB^ZCn{FIEQ+2AD~Ztg&QQ;88mSWWx~}K6xOdh?CY)j1_v*V^!Ap&`0(KwQ{GPS ztw&zN8cklTrha@*51%%!BW~Z zB*yVe&+T>KP#>k$8BQ)1)}Q;r`-n1A#U<`UEOOA4GEWL4LW4 z=+0m>7jGNE^X?UlUY9#krDF4gRL;)FaAuahppvy$)w1Pf?FoM&xhdXZ*Y{zuRKme+6}?8kI$Xr zfvN-3NTH5ghZus;^!l@#9Ta$9zIt^g_AX(|DaAzUuG|0>1%-G4@H+2x_ z@wKVD-iZz}Y<@{#NXUyUD=%NiKSKF~oFm^{0?FvyijwtWwV>sVQ%-Evdsp+)*Sk`1BgGB%0!a|sB96vTgTx2I|fpyWM{Mju$u6K~F+Acw2Kht2p z$A^3b`Bop14kesSzSF@dcEAJqO_lbKoH-`_**{YDw3-)CCMBGyBH;(j%w9I0@F7RW z8LS`3mohPg!>chTRtv@644rYhq$T2yGnLwKd>e zDcU;g0zn^_SZ>X@>vkAQdgpIl91XUOBMp!unomHJoJnUdKVy<)NfaEd9w?mfGH6fV zNFqs0c8Fw=uhomuwrss|yc=-Z=Bsm!Y#%f=G{PLL3aqCTZ;%N1#t?b|o)Qff|+JcoYrqm9mSG&ADr3mqcok$4>; zQPCK|;)3S_=bHtC-@mtgbn~Xox9hW1vT?BDn;je1*NXux8RcYUG9%`FqYmzlb%_@U z-tdt!_9Y=HDT(Po+{A=qVam1W^q!Ev8CNEPP^03KU&kT_amdi^)ZjuM1MBl6B;4Hs*B87!s_jfm{*;$tT{h zwNOoH?+b@H_`|cakCSfgkK4=}HLc*dckjCRnG}998GCncT%5lE)yIBotG-jA0wbEH zN(`NGmF49&YFlF&PsY6*H`jBX2CTLM7;t0rNIo#wZEQr>1osGJaEz&K|#xF?y+dZVcawJ2PjIMxwnb|R8^CV4Dxc0h`dnXV$_l?|&# zsih#Xx=G-}k(kXF;^0l)WtZtP+TGpA=4(f>6ag%$EO4JeZ1;jBvr#xyQ73&3I(^fo6!C7EA<4D zZ`@0v`A;7|a&dA>gzb#qf4A+K1)*>PpN#{XXuFZ5x=&7&eH0;?OxMli65>k>a;&MC zD}hQhhAfp5gIA3j$;ZDlSriDiP@~EF-`d|{C%8`6U;62Y>X?$R zayH#y<#=dljFg7PKx(6dbd(pF(rNotNw3^AQV-4>VW>HyZlNVMbM8ncsYGWy>6qyr z9bGT^{)R)o-eA5UL(H{}I*d6KXrRJq1_uxRtchX7ONkBl$4DF|!xXo%d4dv_nU1b$ z9%zvMWNir(&q`e&i6G2UZT#@z!*GTpw%a%G6B4->H2nPaO!cvzwauyQn##d(j+n9A zXC${A%{gTsN~ku5Cb4Eiy_lo#2w<6ifRDxSxbI&YQE)eT$8DCDevaw&g!5)Wt<5>Z+p zA*c*9RSAMR873(40akKu)GZx6e)C3>+buJ*vSODh3dVoCm}r{H_%iq+YK$*6q;sg# zQ1urU;>$SS%<)PQDvUwfk@~}$V;HCWP1r?PJA=kwe)w=zABKgO3QM0I;CZ-R_=_^p z@tokQ!pzF9Cla2TdL&vy*4(p$o)A~0Lt}VaA_UvVl`B_%S{^(oDQliK)BWyUD-5qi zZ)VEQe9)a-{QobPnU3P#osyDbuvBP5mAEF)F)zGNxthBC=g%MTCwCG}3O<>hpu{?= z6y`lu(kGlIL!uFKw~e@M+cqB&!wYC{c_%uOIVQW->i37!KpJSfVfDN+xgSDoR`|LjY)rv9~AW-f81SPoCU5 z0(sV8{ydydHHk3`Uk^vdF|H#Y3gl$3M(AFwz!BvQyL1|6f)ksZ{WZd7sbd|?zIJ#w z9vaBApvdY!4M=$)@8`TmCv=a^XdU1NYydCOlI)ag|D z4~S5-aL*kPbE`ZeVF4|t{%uRAE#_&Z?rDn38+tmZ_#9%Q3N8kwjcB5oCK&Z_TQn6Y zjO9mOzrK5xI!GhLY?sA8NH>;PkmXwv(i@Bu($j06ftO|-q(~e3PJTxLAd$vyVS+G% z-+z{RLWIRRPRdZx*PP0|QBp$5#lSA^lOYW?GrHl*iGkD`WKSWDu15w+j&L~ha!lTfpNS##Ru#xlU_PNFLZP3DIv&Gl_mYEUEUR~7|tua2iok=CQfE|j! zp@-IGj6EIL6RE{)C}Cv1FT?-qichH=bHF#%G^arHPoLNXnh!JB8%w8W6E(hkbDnM)JPGN72_r2rFk?-?=q9*iSbBzm8ko#X9PGqI~Ux( zeKNPC9vABMIcIK}zv+VZM7F%VX^V1f$eT96moRX5b7vtVvG4|0n*kJ%e z;;8u}7;_$69K96L>Y%Vb>LaCLhT#DBbK~5JUFL?vtH3TcgA00YkWrHs$H*@$U}kz2 zs-+~vIcmkgn5dwrnC|G};-W7>GVu<5)8F3@SHuvmgP-jr=N|p&HsJ>e)1RC zi;L&a>zwlNs9Qtu*maQpg}D7IJG;6LNb*JrD9DC+L$Pd&&Jfo z`C}krSY2Jr_M93NlUH56TwBCq+wG?E9f@$3)wab+2sU2AkFvVDdM|QYd(rkj|2Y3O z$6~z8L^oJ3NLWPVIdIjW;Bjzb@L2pM#X%~1Uwha-pcW5wgg-sl5g;g;Ne zVDH|Yiiw)5Mce%S9fI>{ab++<)j`Ady&N4cpRvpI^vq#4P4uq_H1q9IZ_U$)CRn-bc8EH#$CS8NBi~Df`vh%CF*P{~h8W;`v z#dnj9=S8cH54}x`Lg<`SXPRQOnuJ8dWteq?Vf1W+P1t*DtPysSs=PeL)8r^Nkg~BY zLh=9i@J(7;Lt2m;Wj}oA6;@eUS;W)-8T^tp8#A+?o4fm;>YAF)ix)0bGKjQ(sqT0sb;NbtT z+7DFWga~S~Q+H1fV_!%3b5-=~k<~_2=KQZS15W~~_WAi~XzN^)r_H}K&ea7L2=Dbj zr4G!C5ajsxzI}V{+vvs}nt_fbZTN@8G}J3ka?vW&q2!g&JnKr&qmOUh8hJDxNdBl0 zW}_@E?feevv}Z0@qmn2eP1P)he^CrHP@Yl|>KWg^yE`oZ*d&(2GzA=89m7j z-m0-0%BrrN8LEG;?|5px3r=_vA77H1KFEWAIC2$+2@HYxXQ-j^}C3(aJG~A&3!=F+cW_9i|xD{ul++kCX=vx%zrWtgi6_vyM*@+0x4^>zJ8&3@^T8l)z zdw1BOT8{0SJlR-LfLHJAQ*v-B29O91VonomZ-04#>XnF!iax}slsv&4Sugf%<;SZX z=Oz~|hza*vrSkLgzG203)QIKAvtaWWz$>@GACTdPm3iOn&w|BYq6+N4Ww@sUEeZZ5i@B z(SDpM(BIvy$V_J0;{yI`3Od=e;%JtjoFC~~S)&_z@9YaWgIC*glJ5+Z!p&f2mFNkF zpeSb*1!jzC|5wEQ{K2dRir+Bf;{Qpd_=vFRYHAL)_4lj4938##>puHQ@}Y0oL~cMx z5pd+;v2arZ!Eb=A3~yBOKAbUhx}$V(98oZrn)&9N@(F@rd%fJ;#vn*h zU&3GAHkF-2Oc10~zl+#KovyR<8iw3Du`cTc&v+|kw0i2QG6hEK0-7`on{ zz);lRV1_t}m?Ydmt~j*=2r9`Ni1vdtlmp!( zij_cy_z!^lbGl{=C$up6+MDYZP!xixr&zlOs(tcf@zafp>}e{U7*kfJ*at57bw>wh z+G7wt2?{DI1+N{;SZnL*=r?UG{$zP;WM0tC{?IYyJHyUwFw$~;LpIvUpmjUxiHE9l zRBIcXb(+fKUvi@-Ytf7!ot>JhXzA?i+-GcTeCmDfMDGzBo4>HpZ?6S(zlc%BtL2B4 zlwq(;22CfWGA5?;A8LU2BJ2-$gaiczC+7V`QNeZaPQoE^M->lRIAl_bV@Iwrul;5n z=N5|-$1f)_1TB21yI2BS?iO*Rad5tA$<{jWo_GJg{4ImrF#Y3^`Rynt5$@A3$;JsO zDZh$u-W+z>vSo{_m)Cbine2pV$}Qc?S|20Cub4ozmOWmhPU#f9+Q$YUwA-q_Y#~VM z3aBY%a1M8b1qICy0lw=6-b3)@=(#d7G7@_0)~)`TuV0Iiokq9*>S)2F=dxi#>Xey? zaqbmRl52}|a|4qII4!}TsPc@<;nQgkkB>-&I(-z1wF4R=#Wu$VaUoQFX*;vl4~XGH z)2EB?vlA0aT|-0u*YL2Id-r7j32zn`c|ASt@*4Ny{Xx%Pw`33A=>Ri-y7|i=$xhp- z2}(4#sMh0?*b)949vnOxZIs~1ILFQk8Eq&eCP(1>fA)Ln*_vHQ^Ej)~8@s$aX8Ecz zLox2^Jw$f)(jRkke!cDOk?)6xcV33Uq5Q6Sy5oJcGt3t}j|KvsWP<$~O{bR^diz4> zO_*8M^>y6XLv8r4K}}wMX5E1UU43LCi`>N*jN3cfm`lu2f_DOQxrinD4Y==})Ju1a zc9!|(C=S`h{#=D#L=wN8+Ii#hcOY|W0uq;7^e(1(NKJC}i-ckBq!|uDXwk;;3zUfs zJGHc4Lrh%hlq+(rxiHM0+k$)$(LcXy>gocj!^2rz4yYUz-BBp`4nH;r51y@g{8;7C z@#Aaepd{|Fc8q%%(+mzLCO20zFfg>@&)&@kvg#X=veS&z3CXRCW+wgI)%$e)7e1#z zs%B)3iKp>hUc0!J2BgpwMA&!ndAPePoEW;&cO@auVzPIx-y`(kgQSFnvhJ3asWTXO zlA@!@5x4enoFUKcECsXo$GI_tMgE>uQIwRhGMc4al8@cNgM1@xZCgymj0I(hmvW-N zi{waam zQ|DBV^och&AM6U(a(FcQO+zqy49oY4*Kj2_SU=ql02_|;EHTpzHvXo7Ua7>zoN@{X z2#`N|DavS536_5_5EcQb2Uq6k5YVrpI3-9CB%7-sN@_)jF;CKs+Pc|^y zH~jY^vw4RoVG0NZe|QriDXthS_cyHEGx>m7dvzI2xeQwLqcCkE^zg!qvzKX?dM2ZS zd^>J0bsLNS#t{)*_V)Jc;G;88-Y$DTFQ`@&f~eK!{rwlC00tYl(MCt@Z36=Silef< zdRSbni*o^92xE;r`Jr2BLTwkGPw0wRYumM|xCu6zW;ppCDk&;b{ls@^>jaGx<-+ai z{mPC-X()4`Gm;Ms4AiZts5s9!++|&mzWeP=Lqh}7yicm(O=3LBg77l%u&L>b%SZRv znfXd5o&!nc7a;KKesK4q2(Ge$;6 zmdHwcavFzO{D&HCv$y^fr!X#K8EWQoPI>uHrMNbEn|ePVne`EWMVn=fw8@s_P1nqe zMa>V~(yF+Edwk6aW0rh>y0K#VXCt3~a9l;byhCvqc72^$rl_LwZ$r4j(S4hfR3QSI z`mrJ?H)nonjG3ZE+HMzv8#rlFkNTW!}1RcRUGre6@%NBq6>@sJ&p60 z&W4ijj{=nX3zqV1X_Yj=(25dVgcBYCd`L zLmrwObu--jbSiIfkulNJ3WP;PX*U?1(ujE zYyUp&Iw+~YzW0udCI|7#h9RgC=Qxkj)8F^O)p)`BT5IU{{POao-qF#~LEto04C)OP)ij?B+^R$e4+NqSOW|Gxgo6Md>RC@(?p8rzlJ zT*;B(UHEJt()`vM}Tq&IEioH+qVN4G2>}n?%=LgVYqjPe8HU_X@+Ru zGz>nM^*Hae6ZvI{8?zKnT^ofcXAtsxM>mxrM=**x_mf=G6lZ?I0rMJN#p~lw{Lj1k z`u;etEw*<2!H833b@#q#`YZ&3VB`KYg~sY<&py~nkOW~fOF#vVrtjbS#t?K^kOMLE z?jW&efjIMKWn)W+WaR*^@4ebue`8FzOSo7;1%m7@$&BJtTH*P8kChe<JmT8eMID944zST&!C!f9#H*@Yd5_SCGHkAw>;QnzK}GN-IX2Tznz= z>m!=^=644f1U+L4sIjD6+c2+3GobNF-QO3qf)|<>n`UF|x;Fw%zp%BQSpQtSVZpFpL-3P<95csF3LzkwTBRw=c zoQ^i~uOb@q9qS_k3Lc!9r#2`Q*GU=XN=&dg4ODs!|3e$H)S0Q7L~QXRJD)1CW2Mkw zhB%H$lqekNmtOKcNQffJXhNk#wj=$99XioQs003;hQ;#m1kH~4sQ#I4D+17#@pUc& zdDqH-9P#((?oHMEa?WH*29d)<*xz<{CsbqnCZW=wxs{nod6`GSsGT)EHFfe%L4gQe zcesrum~R`C6Eg;4L5oun%CHE00Qo3|ZDR!2pLjJth#jK0wtFQd7$%qJve0<$wRmJT zw32^?7K5uuloBU~=Lz%k8=m#?8Thy~wtn9wk5BjC>#nXH8aOM*;{Axg)ON~{ zQn$Zz5I&$1;XHZwu?N;=Qj?f*2gV^Wzjybpx=UBw2Gf)OfsDd=c{gy(P&-}Q(ZE7G z&6`hp{OG(P&7e&B&X-T0)V(lB&Ep~{n@abjMfd>B@u7Qlze4Zw4?N!`EltghS;C(W zD&kA9Pzc8E)bn;^WaTE%7;oejm6w?*|2I3!y5zcd_wME!s0W$IWPzZj=rDEYaY*j9 zluaIZWV~wACXsBT{8QM5!aR}KV26_`xY5!bywumo>?DIzY0##B2B7V6>EH5v4Vr>p zYzeAJ=w%bwyl{^~Uu;=pjoVK^maXc6(CAk{$Ll}%edMC!GL-$%zuC;+kSH?f=q1YT zS62RFxY_yi7Z52Agv&M|$t(9W=N~H(h5`zCnegCPgy~&(1KGhevUP!3oeG}D=A1)^bWLro7M-(` z$2pK203b1I#|ZTdY;QAEER7aq;5C<8=;=j5s{;I?pacoc>c>R+a#Qz4s_)rad`FFK&vTM z$-I}aczVq@lcT1XSC&}Qq*L_bE&5sW=H_NE2;tNu0+uN(`2W|T-u;&Q6+~O}% zb1DE`tcSCnYa3Lqr`NZ$<~sinzdwck?r^)4atR%Avnh9pd50rK<|Z6#Awh!|NxA)% zpcWt-2Yr2pJyga++J8ulmh`Q|sx)^$J42rV-dA6u`p`B5JvsUcBdWdSkjyzozo^H4 i7}XiFn}j#4G4$NiQZ3E<@(L%h5=;&E?E~Ia#$$< literal 0 HcmV?d00001 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index c4f90cb570a6e9..77500c20be5ecb 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -183,6 +183,8 @@ def get_std_params(candidate): # FrogPilot variables params = Params() + ret.lateralTune = params.get_bool("LateralTuning") + ret.pfeiferjDesiredCurvatures = ret.lateralTune and params.get_bool("AverageDesiredCurvature") return ret @staticmethod diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 24c51e9ee2a30c..385e6ca941e82a 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -121,6 +121,7 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): # FrogPilot variables frog_theme = self.params.get_bool("FrogTheme") + self.average_desired_curvature = self.CP.pfeiferjDesiredCurvatures self.frog_sounds = frog_theme and self.params.get_bool("FrogSounds") self.reverse_cruise_increase = self.params.get_bool("ReverseCruiseIncrease") @@ -631,7 +632,9 @@ def state_control(self, CS): self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, - lat_plan.curvatureRates) + lat_plan.curvatureRates, + long_plan.distances, + self.average_desired_curvature) actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.last_actuators, self.steer_limited, self.desired_curvature, self.desired_curvature_rate, self.sm['liveLocationKalman']) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ab65da2e1d9328..9d2621ab055e64 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -16,6 +16,7 @@ V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105 IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors +MIN_DIST = 0.001 MIN_SPEED = 1.0 CONTROL_N = 17 CAR_ROTATION_RADIUS = 0.0 @@ -163,11 +164,12 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): - if len(psis) != CONTROL_N: +def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates, distances, average_desired_curvature): + if len(psis) != CONTROL_N or len(distances) != CONTROL_N: psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N + distances = [0.0]*CONTROL_N v_ego = max(MIN_SPEED, v_ego) # TODO this needs more thought, use .2s extra for now to estimate other delays @@ -178,7 +180,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): # psi to calculate a simple linearization of desired curvature current_curvature_desired = curvatures[0] psi = interp(delay, T_IDXS[:CONTROL_N], psis) - average_curvature_desired = psi / (v_ego * delay) + # Pfeiferj's #28118 PR - https://github.com/commaai/openpilot/pull/28118 + distance = interp(delay, T_IDXS[:CONTROL_N], distances) + distance = max(MIN_DIST, distance) + average_curvature_desired = psi / distance if average_desired_curvature else psi / (v_ego * delay) desired_curvature = 2 * average_curvature_desired - current_curvature_desired # This is the "desired rate of the setpoint" not an actual desired rate diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 269eac650c9f43..5e4d199faa2ed0 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -51,6 +51,7 @@ def __init__(self, CP, debug=False): self.reset_mpc(np.zeros(4)) # FrogPilot variables + self.average_desired_curvature = CP.pfeiferjDesiredCurvatures def reset_mpc(self, x0=None): if x0 is None: @@ -71,7 +72,10 @@ def update(self, sm): self.plan_yaw = np.array(md.orientation.z) self.plan_yaw_rate = np.array(md.orientationRate.z) self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z]) - car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car) + if self.average_desired_curvature: + car_speed = np.array(md.velocity.x) - get_speed_error(md, v_ego_car) + else: + car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car) self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf) self.v_ego = self.v_plan[0] diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a81cb33a5668de..1b22443e08ee91 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -234,6 +234,7 @@ def reset(self): # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.solver.reset() # self.solver.options_set('print_level', 2) + self.x_solution = np.zeros(N+1) self.v_solution = np.zeros(N+1) self.a_solution = np.zeros(N+1) self.prev_a = np.array(self.a_solution) @@ -440,6 +441,7 @@ def run(self): for i in range(N): self.u_sol[i] = self.solver.get(i, 'u') + self.x_solution = self.x_sol[:,0] self.v_solution = self.x_sol[:,1] self.a_solution = self.x_sol[:,2] self.j_solution = self.u_sol[:,0] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 648bdf74e6a4c8..82caf03b546e19 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -56,6 +56,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0): self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) self.v_model_error = 0.0 + self.x_desired_trajectory = np.zeros(CONTROL_N) self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) @@ -146,8 +147,10 @@ def update(self, sm): x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality) + self.x_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.x_solution) self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution) + self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N] self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N] self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) @@ -171,6 +174,7 @@ def publish(self, sm, pm): longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] + longitudinalPlan.distances = self.x_desired_trajectory.tolist() longitudinalPlan.speeds = self.v_desired_trajectory.tolist() longitudinalPlan.accels = self.a_desired_trajectory.tolist() longitudinalPlan.jerks = self.j_desired_trajectory.tolist() diff --git a/selfdrive/ui/qt/offroad/frogpilot_settings.cc b/selfdrive/ui/qt/offroad/frogpilot_settings.cc index 5dc2d81b79e408..09cb4b3e6b45df 100644 --- a/selfdrive/ui/qt/offroad/frogpilot_settings.cc +++ b/selfdrive/ui/qt/offroad/frogpilot_settings.cc @@ -19,6 +19,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(QWidget *parent) : FrogPilotPanel static const std::vector> toggles = { {"DeviceShutdownTimer", "Device Shutdown Timer", "Set the timer for when the device turns off after being offroad to reduce energy waste and prevent battery drain.", "../assets/offroad/icon_time.png"}, {"FireTheBabysitter", "Fire the Babysitter", "Disable some of openpilot's 'Babysitter Protocols'.", "../assets/offroad/icon_babysitter.png"}, + {"LateralTuning", "Lateral Tuning", "Change the way openpilot steers.", "../assets/offroad/icon_lateral_tune.png"}, {"NudgelessLaneChange", "Nudgeless Lane Change", "Switch lanes without having to nudge the steering wheel.", "../assets/offroad/icon_lane.png"} }; @@ -37,6 +38,10 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(QWidget *parent) : FrogPilotPanel {"MuteSeatbelt", "Mute Seatbelt"}, {"MuteSystemOverheat", "Mute Overheat"} }, mainLayout); + } else if (key == "LateralTuning") { + createSubControl(key, label, desc, icon, {}, { + {"AverageDesiredCurvature", "Average Desired Curvature", "Use Pfeiferj's distance based curvature adjustment for smoother handling of curves."} + }); } else if (key == "NudgelessLaneChange") { createSubControl(key, label, desc, icon, { new LaneChangeTimer(),