From 70a7318a3a9caf9c8cee5f4f4b4676148674e9d0 Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Mon, 1 Dec 2025 12:00:00 -0700 Subject: [PATCH] Curve Speed Controller Co-Authored-By: Jacob Pfeifer --- frogpilot/assets/other_images/curve_speed.png | Bin 0 -> 14838 bytes frogpilot/common/frogpilot_utilities.py | 15 +++ frogpilot/controls/frogpilot_planner.py | 22 +++- .../controls/lib/curve_speed_controller.py | 111 ++++++++++++++++++ frogpilot/controls/lib/frogpilot_vcruise.py | 20 +++- .../qt/onroad/frogpilot_annotated_camera.cc | 81 +++++++++++++ .../ui/qt/onroad/frogpilot_annotated_camera.h | 11 ++ .../controls/lib/longitudinal_planner.py | 3 +- 8 files changed, 260 insertions(+), 3 deletions(-) create mode 100644 frogpilot/assets/other_images/curve_speed.png create mode 100644 frogpilot/controls/lib/curve_speed_controller.py diff --git a/frogpilot/assets/other_images/curve_speed.png b/frogpilot/assets/other_images/curve_speed.png new file mode 100644 index 0000000000000000000000000000000000000000..6ef164408692fb0891648f83ca2dcebb994ee3ef GIT binary patch literal 14838 zcmVPyA07*naRCr$Pod;MH*Vo41rR>7evBwyrCNae%w%ComqoT2)VlRIMyV9!_3knJr zY@mYC*fsXvdoPK-qA@5P1QfpS?98$QvFt9h%a)nvF%r3R=gztJH@BU8UJ-;#uz+BJ z4_ZJZ*ha8`V1W-?KmbH&9fAcuVgUgVp?L@v5C9R{fM5ajEFknC>e)@W%SbFB0HOmG z-S4-`gCZ9>F3&4ek zsav;W!}cA%UXY+q-6;IN$QBR)DY6OG%b-OYwB5WLyJUCg0Wbctz^NTJxOLYAQp-HF zbxB3lI++l$H6(i80#03SG(U3Y#Ub^cDBPuv1q49Uv04%DY1RrYw(iA#%I4uUF&56S z!L|ctFf{>8Ok}*^?OVXn4wxF2jOOj%$et*QZ;e)+FE=`U{^f}x7D2dONec*oC}}9Z zk~R1hzwSPQBb48j(-sKvvcuKC88F^=y?Y1DTbhhE9T`YW;@hJ|#|!n(T>R%WUu6>V z`LG29K(yF8>NZ59eaCT32FDo*rcSWO&3h~yd4R|S5KHoqmj`5Ji6F0>oVP$kbP`&& z%VHtvh^FmN);)ja)mbeT-zXm?0HUR8uh$q2_a4P@IUGS_?Q!Q3WOEDxqIrW{jP*=I z zpsP*OJWTY=K-r(t3YaK%kquTXmf~ds^UrUxfvC77G;75|@~i_|bUH5#w=~$W1VA)s zT*{g|=eT%E0-urgc>0e5j!w?m@b=Guby=p-7rtT_+F<1pDgNO=vNSFUP34fpw?)g& z7lnzthU!rOL_>z9Y`Lpd7fwpx<7SU%ayUf5Z@n`SG%MwkC0G+_i|8oJd`M`xwPZ~a znzd%262-TIQ`c+467VN2sd@qd5Y=s_)E(S?j`PPQ@EgVHIYdBLmrMlD{xlr&hpmaQ zMf6eyBs6i4j7~xecH%C2-V9xO{?&H>vFCe~Dxi=^DGLaIC}k$q5^;CP&mXfu;5a)x zidPJ)e(R%L&r$fPs|k`QOT2u={Dzi*!>5$!HOkPB)n#_pcSxLk;!P^ zMGi@Q3>x;d_xkNm1ph6yt*p3F0E7o4%CqP<7=t2~VLAQ(*+Z5H@wLV6hwM@=0G9z7 z@SUbU9P;O^inPIsMT#Y0S`C{VngZvZ%o4H5%U>{Zc@J*V{!;)QdXz-SYh5YYuvcQ?2zZ?0nV-R;4?c3 zR;9Cfc$y-MxFyzD9A<^Y6b2G=Ghl*W8hZ4~g4pIQOkVv2Kc6hmu&BRh@%msR%1i*H zh?VUVgdnd#1X4C9cUmJX#7Y52F2Mne@k&GK@6(I;r1#&xZiN*hW?LhPg+y!)3>lh* zA?|4q+r5R!>+dl=7{Sx$-we`70Sr7-0gxh8@Z?#TId&2}D2wCUtTBHM2M$@!0gUoU zMcE&iVUMP+jjNeniy)!-h`Bl7>Y9a-6Vf2De+yA!RfNy5nB_P7il?T^V5s>DfT&rs zp$o8Js5{*0_9L5YuwbqjPOWBc@4uMFaGxRQp^mnDF8{29@+4k#msv| z$zLIAo^8a0;ESWwd}X7&O#noVYL8rj<*oxUh;H5gyDcKaEoG}!vN=aC(6tRP+#>~_ zf6uHcY3tgv)e=*^Y>}AE^dx3xK)2qR@SVYINQzP_Va>u)D@J&q9jvV~>HBj8Kvb;3 z6>*5`<^mVGZD))f7DZa%{u2g{cez0KcEB)?WR(Aw*_Y7vb$E{j0>;_lbrJ&!t!_DW zd4s9p$qXcE74gUN&*KJ-J<&(oWz)y!34o|jYjK;f$*Bvv&@G!5+aqd;1W#Tl;BavQ z274sov#;Opc&Ngoij?%!VF|q5?PZ&Z1PQ_0qHPxDE=^(}Nw0uyF_kv;aXb23ksf3O zHw%C$S;-rA;Lp|_(4La7nd^X+(PEkNAT_|Z?|=cn0Rtu`qD&=zmvWUX-hYx^I4y?9 zFo*myQaKwo$;P5sc21H}9=q05-QHvP!A|Nv!x-)p08yfrH}A#X7Ol{Vl15E+z?wL5 z{t$-X_+M|J&$vXCte_ik$Y1IDB{9Yia*%lto3J%qt8O-;H?fdpeue{UtL=6kytkba zW-!Vm0wABL_U#97ph_1?JEOvT9a72aVqJGcU5L@X79P(HE_k}4& zyOdTdWE<`5eJFCA}Vgdun#Ty@(lVp^~w&*ItK8X;>nTET z9J!o;&ptOCIOO2aaLcWSVopN;USOXjts>S$lrd`GEu#m7>v$L*;DB{&#Yknn(`n9e z@O%P3uV{F1a3}5~x+_+<=-(SQ&BkK+j(bWaM9pEiZmM*c7 zh0~$NL9144W7AQ+X*x{I^?O3D>{0@4CXVb?B7eu792`4#<#+)nR>h1UQ=>bXXq13> z1t8ET7_uX%+oEl;aK0s;zF>FAbAZ{txoABw5fWS7-XT}E@hh8-+)~{7I3k{n{z$I! z-TGw0k3CO9$9Mg_-x<{zccTM@8(?AII_4xGo#&8w<-P^{+yG~{*RU#~o4!@bHg zk%Qxy{t7Q(3IstX6UX_c%8u_+Ac%=$(~TxtictZ=4b5|Gt2M%BS>e`w=DY-X<&GHu zxQtDNU0L1o%2l_qt9u{0%1;>Lkbj-9waY)_ zPCdyu-!!-mWkML4yr>Ts*GC2&3)tWR;Rceva!iV_sWv!sg*mHD0Tw7U&j{~ieEwbW zoFV7CQS+T21%jC3X`3G{g#tl%Pf10OeryEC7meZE!gnLqbbj)XLdk4cVfwfP%~r$$Cg}yIF1HL)pTuAR;fmK> zKVyN=06Uzy$_%T>&AW9Yz}+hqRcdNBsGFu6ueS21;B;Zrnc#GE(W+m_0!37L67gx( z`HI0QT^S@GT-meh=PWSW#}4POvl`@iK#K;zD6dp}Ra<~V6~HLWY2<8cnYRI5j_OmI7uk z3Ad6(m8XkV02i!OC|#L6>wu;mP8*CwwLt;Gm0G@bPJ$`p?Q!F-qURtu#(AZqQtg6j zvijIii?hsyC8{ZS#iG>;%|w%U!_jd!7(lo^$Ca}Z1dUPXkW*u#VbuihbX2G*z`+Mx zB;vDju|nyJghiubx$3(e(W2v7y&rBF1R&fw$NO~1TQ|wYM9()U_jA$Jo%m>6hLQ)# z!9_<&^EDIyqvIam9xXec*XxP9-UCuVYWai&zU~FpWIJ`vg128Ltjh_TfSLf4ghgXG z>bnF9NmMs)BbTUd4X3VG^>ztZuL0ribDTRSf$u1L#J^+(Gu4n<)3LHxHxu<15bh57nIjem7-NTLayS5}E46G44O$cLxF(X3sHS+u zbkW2;d|5KubYwgfL@%14x9ff9?fakTl|;2(0>bsJI(^UrlPB5X@pJy9mNn6Uj3Lk1 zg^Wb?OBNDZ0-m)n8BSeUiRynEV9?O`-s`tN)EkLvy#s{nm3wl(C1&{8;=xn?rIwAM zL2Ke2V;9+A`9dlF<>lGl}^BgDkNHHtW2KOGbaO455 z!!qC*^!}Bq7tcgEb6kWs*?F=v=gfcd|0>W{XhV?>+RFZ>Ke^1`>%&Q%103qC1Mf2{M-~j)XT|#rY0J@x_QPevBA;>QY5gd z*JKZpsP5TcE>T?*6UQg(#i1*`0fc)J@c2$Egif`_?FR~}<=(xr;XC)kI^<`Ln8J6A z32xqDkI~8-G5yVLk%d}C9uYx+$btDltNAY=92yGZmk*u{v_iniE2IEyobs8O0#XYR ze)z@|t-I%7uv?x^Lh}1kMxy#vl0wIwB&z!jV_oI-M50QZ2 z9e1~hY3SEK3u0T6sQv-d1M`BWN8Ak76`K|=ht31SjXrX8i#5V$TPfhkV>AKXJyKD! z>c<@q-(`!iag8ZFC+6Z+qJGBJeP1K8vJ@dcEiZpKr)v#u^BOD6on~F&(3L((RO>V# z+_+VTH`-u+s1$$8hgG>ij}E~9CZ(dxC*Y7bn#;${5H!gI2TvQ;ne(J~%urKTuN{9# zG1K3Mi8;pVxXS`Tc&9<)kfYNQ)jA6ZH{#)eb+%X#W{Eo-spTH+8L8#6Rh2*Nl%8yY z@DLGZ&weNKLN$~t@hx)*OqZ{{bTYA!M76VXHUh(0IXE4asMbk9xUq6~ueL+vf`U@Z zy*dCxJ(5xG8`Y1=ZeM2#e-9DvKQ@e*R~8l`#HZ%TG|_eaDlAb=Wv1YCljpG#)u~@# z!;1281IM1y8HsA01B4r8ecMudELtRyZ34=5$a{BWq?XHlrJirq{fiPz@fBg?Zq?z^ zwFXGTm8s*c;XW%(XXhY=ZFZ6*s@?4IDp8^1PA(~PmdGWlKgYJ1FE{mZJFXKF)j9a57qN8^|G;igIuDTza2fX+ z4(0f1&{=Op@Sg-Mj?ljzy0> z!ZB@(Bew1^mnl0@p~zi&%6Oo|@B~Qh`5#8yIBE%hpIq!eq26R)nabxYjhUGUG3)ct zqNNB@8~yJ&3fm?Au_@eomBejY#%Cp}zy2-n2&Bm(D>^@0aMXZV_yTZ-;%wT2mQ#XxP0O2|w z&h~b|#y`YJXMGF_j)NBxP@x#5mIt{28-7>UgqpptN(ECy#N?r2GtCb#CS~!aAwR4! z{iGNJ7xM9inXelW8L1jC|%BYdXpQMSqDcp zKp=TE_ZggliGfTc8%;=5^9~3%G|%D9)|fxrD*u=)IUafp1G>8>!n(BX4QINv0S@df z-X21gr<3w*&}cghoty%R)Hp!7cv_5!LmgxRvt`G3%>dVaSu%-giQQY?C8~J^gd5cD z^e$_Jg;+5w5UdXQh(3Vp_(V9A*F89>VfN}I!tOuwWNs+>bTzAIfw+BdAhlA&=F+F~ zt6J7mMdc%3EL}1XhyE$NK~t+ zoZzSnOz=ua}7}SH`zKGVn!h z!;o5D5M++2Gv6Zfjo~z?s+CQ#X>T4XRME4YP&K-imEks6wTz?VPU^dHNodxJ9d5-t zp=IZ@pPaZW1qgSw>f%WW0>;?mA?sU3WpC9q7n3HvLAmON0f)RkzWp4Cf2Kz56uN~R z3^Ouy12m^*5-eT+4puyeQ7XEX_|GAFzAaWQvy??}r0c&D)otO__3}rTfIkTccc0_z zF$n@j7t$ej&VskEVd{`~YGj5ZC*KxHaHo``3EFiK;o=z)uHP>H%@ykL!~AV9Fxap@ zW*@?9)dE|@EVIlP7Ns*tkugbV)m|=9-4b1T{@r%}u_t@pm&Zo|;qH*1I%I*sadrwF zawCvh-WO+qzJs&W9%j+Gg+PCEjP`qrv)iPYxg;IuPl=F{R=mB5rKJd)cLEKX7H>KD z?=#+?{4$eX10lYqICIDs&;|Bprk;%IP=MP!J)x{QcIECE6`wjoF)UvV#)u(h1 z&q`7E*Gy!pA(b%2!vbSwzTZJJ;U5v=7Mo-DcHrE#oMPFOBoE-@$IVfthH>;F)b7?s z*&rfRjFdD+ap>F1dHCblOWDs8`_~*5wd!I7w|#yRkO8eryf9zNBs{}-Lp>EmpH$c?)1WzA=M zi;j>WG{j!Evz?U%+)nsMmc8gkZkz_^OU^K~k-J z8QFM>!cN_h1Cmq`HV%(e1EkiMWn{sm36Q!qaDxtuhz+`XkWJlp`_O$)FIl3kldTMp z%s0T@S8NZGPz^SvuB!&fm~N$T;FO6hGaD-)dSwQ_X=wPXSA~t+tC<8l4=dM*=wovG zrUYfaRW3dESudZO;=m>`R>i!-`Rm%5o0KXA+%uw)vvlnt|7|T%X>lil!oPnBY}#ig^Vp?K8`cJV{WDOpj&VU!t-L87 z$A7vJ1&^`DI3AWy|Gpu@bUzVx@6JdWY42q@nW9%=}BGW!@HzYka@?=%pKUy^pEa|&~o@+cg#_`5VT1`bOJFgGd@jaxEH+`T%%dFOU)Eqw1sT}@iHMVn2#u$ykWeMN#0 zPkWrb%1qdE^MHo606!ywe(zM zVi86UmB2s3@K&?Jr!a7&vxi&h-1_{LNodrZnImRC9fmR$qD)9KMkPQfag!EkwtYVi z${@Ka!L*6?xNw~VNlm6FsaV%IdXj}6*6^N__TFB^LM+1mlfX~4K0QxSwW+V1VTqYx zS%`nBxna=H)kQdViT|>z`mTJYCpdmzZHY8?L6JW0bT*_xQwEN#r~S~W>&d34&fPuo zzUO|@o0o3lo+7+`Rg7s99ST6wfF|y~8K_*}@aH5cFHI2_Duv&?G}(lm=4dqREMaeg zjeB!Zy%wL)g(FDDm^Kj}!9w({sa+d& z4xYmq`uWZ^F+w~XaQd=h;$E*hFu^+m-~Vd(kjRZn8+;O54SmwzQ*%vfi*WfO({u5C zSy`H3U_S|_FV>G3W*>V0$PgQ>SuRByJLe!cmj0fIhVq`{K`*rIu)o&TTM3_xU852p zg(10nU4l72_BeWu3EDst_qsm=<2=*xV^ibt66L$0ek{LD3#{NdITQaR@IM!!I&9KR zgf06uK%bV3w0MpUq8Cf^70l>EvS>{b8nYAfHxGKEmD9nRS8o1$Ntq(43`pVO*1hWz zgaz2+z)3cmVlGhUXJD*X8h&o7|6R2*M$~4SoGcOQSI@)kzclmII`tu2Xwb)NX_z&Z z{{IJr_BrUH3HgHPBs6KsOvv9n=z*3U4l31i$lqTrKW%d;x}xT*RQ)Fe zyNeMOorBbL{)a;K8UlNdt6#R_xHEPc6MB@+o69!=r-dh@bw{QvdGn|ToVuL&=xWu6 z%3UNtC{ylc;=^l}h@5VVKMyG&srwT!E-(c@*4OWeUvX6Yhn{(4wJnBDNW%*aoJRcq zD>FoeTi~bm`Z0(6W3?1B{A^^tRRo8%B@i$x1?{^s?d{FuZs^?YbkkGkp1yaV<5Tro zWI*Tv?K`6L@7o|#_h}G9#d~=a`}=p^&8GCnw>npHz1N=f4@~e~tvm-P`=XN978pm=P>NyMekrEZ)7% z0$y%*vcr6I*;)hReACda9|LyQGiUT2aLakm{=fg@U!{uGcmsqU%9UVyc)=Q>)2#AW zx9COMGzP{^NyHabjpq!sDwUy1T1^2`vAh|Ad?om8gzokrhxbY_c8Gm`&tYc+xJ^ul z%MfN*nDyKVLx()*y=CW(|3oQK#m1DDS3oF3u88NoGuD_j)e6VXGpkz!Ntaf@sNh7D ztt3FAYGrZLYYLDrJ~Kt2w>i4I>*jFy)KM`;_I1d2w;?!&jY>y9Hx`Z;?J&yy#o*sJ zUMg^|LtR_*4hR*5D+B%4Q5(#dD#iJ03NcUD&cN`1*Ct2_RCh7G$DD^$~9*TL?$UpJ!=FIDv(bQ zg1iC|NIzfMXNwurEM>8^$gz)B97gs5`g^~ET`7KD!;4}JeY#hZfCTx9@!LqCq@g%JVCe$@yeLE_*!5eI|#t}2ZJLWK=ofCWR{;Z8sAkF`hWTnU~$XCcV} zCXWR=k4k`rjR1)%m0RN*%`EfM5G&YEcK~_(zzlubl|T~f|0l!Rv6TRc zvK3eTYvBG}Wm7Akjwb2UgXqE#EB=WodbcZyS8OOYignz&T{c1%G5?U0R0B)ql#TG2 zwriB~Z>U;ItpGwrS`mxbZoSc)eqI~qfW=W_yh>sr$p*sxffhp&AeIV{C{u;izXI;x zQzoTqX)2U6MUYSNuVVcZZ;C$cO6CWfCjHAM&9V_5!@%)2r5aYwue5xE|MnrOy`gHU zwFU?kYt061?DQMD($C9hIATSNIg(SEwPuP>6cGqC8zexYOcnlA1$cO0nUt!f`MfN% z%-eHx@vUOLd~OQY_9bOf8+3_Gfcgz`uw>n929DHk5x2N%th>k7zN)>Q>Z!E~2o-PL zHf(Fx5gqC0g@KM(v(_A`?4A@sGM{>q!NPFsQ&st6W#G|+Po+}*_vOo&Aizrum$AhI z2=yHOol4>v8_SJ`;WdBBLG)$@j(2HaVZ*W?Hw+!Oxx4Cbr}`aQ2ZV~aaTj*AYKyk? z^L#%?tX*fGzd}JCnRC2>)L0`Tl9|IMUKNfY=@5~II|_5gWs#|*=}m%#IX74JiEto>6C#~0YKy5>&T z5nDR&|HdEBu~-2q+>_A6ebs97{M9YmOmyo5^qC;Ir4+8lZ_5Mm@%%TJ(j`pbH&KEC zUfS6x{QJ5&Mz}cQUxo$QRlRl&R&V9PQDdj;h^<~S&14R$~ZdvKAwy&E;pMg+Uh@h-J8wnTrob>QefwKgGt-$zDg z0HMNh_esbtB_cwCI}aHLNBMaYH$UOf71^qCDIg(%uYN<3k2U}~x=(^hBkW~?r0Jpn z3$5B^Vdi4SedKM**VwqC>V~1?c4%cO_hVMnDL|;8+#Ppva10N#$oB#!NSf9KM)?WH zcPo|v5;@~`zEGhMs(zCs7_6ZlWY0DW_>QrY1z4bqe5*QkeS^R`%sA_9@^@Ifq;jmA z=bswi1XRXmItK_9n(MnouJUtdTjJa`MMxn!<38HYxC23a;RqyYKrWI1@t>rbAr_q> z51wQzJMKW2)Dr04CleEc8JG39Nk1bxqTKTF{<}5mKB5d5@-#XL2o;X&1x$yo=FOJk z7)5X-IckdF_!BVN*LXt;Rd4{3nGp%E7XcD5NrHhq_aN(Hq?qAvQy{=X-ysA|?^ z<|Nj}l2C^TpK1Fv=6I-%p>!4ydKvC{5<0#cHdArNogk6L-SRQc;B=qc11aq4k2)3q zp?e6BKo1E9@VJT^qssX}hNM=n5J){wr^`h8ErvafC zZU>}j z01#Tm3igt~)uZTuM9#Lx()m^l9C<)V2d2k&?!$Pe(i!pqpLEZK%TMxpOqLHY^a>Cv zEH_#T1*eOcVWWuPm<&2If-a_+e0t~$bgbK}#;79?p!zbG* zA~>>2S3drRF9)ZoWnOYMp&mqCK;{NBQp@r{(hj!3l(1wpZNmnVe)b!N4ZG8O%g(Ds z4(g^!F!UA>DmXV5Fi9`WC4G`pY)_I4G+@yT5d z+}Vqg(5MBg&+)V;TnAn*ayU@_-}=Kj>ijC!hloJ)EybL*Bf!iq&_bYLoPN!3-R79dZiI6SiETCY$-#1uib<+8jK2SM5O3 ze7&P8`RFYm^paeC5|V?PG07fht{8`o`&VUwm+Y}rRVw^bX$g=(TE$WUkoW13N3Kpn z{a@Lh<53@U?tY@_sdEq2k|6k0nKk))y#|B~!&SN>VbQ5v_1!$6Q7wSxBo*rzmau5m z^1yQqoCl!;Qvn_lxK2{82PrJIOfKsrwOp^UTxz*5oVpxROT0#t4PV*3^&Sv<(N9TK zH>?eKk&b&k!-1q}O%uF$CPHp5Go*Z%nFBAAE(6H{i6W-4N$gqWC! zWalrq4S;T=RjjdY5_DsCxj)vBNNPE5rDcB54O-46HQD;VGH_%*=z}&L52!4ArflQY zMmmE4gbc-%sJ?wwjA;`K>bN(o1x)hIz}NK+1Ckf64}I^UEP}Cm5y&4>4&_Sx|OQw`=UZ^uyS!h9r7iyT&d+=XytT3 zX{lvRm$xYS8x$b_8E##XV2-anj$bIa1WdkLKQz)^ftaFHsHX2-KA&+PNoS`h)MG`& zB%x_5^`(|IUFM?XZ*YM8XX1WEf_Z`VIC6&5lhkA+s;dc*sML017BN!G@{@oxEStMD z2`$^Pa6IgW)=tOudd^YUAO;Bty<~wU;LFU8d;Vr(9#FpqFxEE})f%YpQlV1o((}Y? zmoqNwnH(MR84HrpwiB~Mp7p33I&?Xux2shjsvd&{gkHG7X5uAF(30f7)o)hgOz=*W^@m>CH+lKk0&Ay)G9?YFP{I zT-8U-L^N3N^r3_)2B#rmfcKzn+x=43%E~tjZ&W*r$qI|vtkVH?jSn}NEh`SB(*#=UuwA} zCQV4u`ICTrv14?Akm0-n5a<&OS&)F62W>HRswJ-6Qs}r3{0$fu@Cx>2j7y^W<`r|e zc6OAV1f+}R9DN67!c8u<{BKhPcxUVM8FIeBkmqBRfY8fxLkeBoX@_akE%4WU$Q=79 zu-WK-KtJyUSUVbrX5!ty&Ct7D3E4geU9?)&qxTz(pUjn7Hq+VYBYcBl^nj27xWVa; zZ?H$mObH%7Q|P$+{150cApw@Qh7AcxE%$6w0%@%GjfDho?v@4LS#qi6x(J(Qt1+o% zO*L*l@-^BXL~cB8nA1Hm4wy4fjOQ;EB0ke503F>DAh9+~NJwhAXRDI=I^<$8(5^!^ zrbck3mOqQ|nf9knMLguA@hg*u(E~z8;s(s#yvPCJ5n?1HDs_QHC z6rsb=blm@WZi+r_3+j+JY@CD0Sh>{lcZgg3ZLGV;W}S$#&S#@nB`*OGjA15FzXVB`FVJ#`{?18cQp=qR>X6r{or9Pyavk!@*s`+5 z)`6op7OS$8CK|gsc?y8M$7+QostfD5&-4J=4oiTgoqqKsg{7A1)eGp5%cPcT?dUgh zW3g!hs?&%yd5-|d2dvALs9xaj2-0y+PE+W(2aW#3#HM0VlhtG+FG}75ARnndb3fP0!J2JcI7n()`#R)BX}sFFR{-P_R_4x0qGvfEW|df` z=}oI!wBtUsC*V3E5v42Yri0_v3u27v?~uQgORrnIUN%;46U&Aw1PM9yxpV{uc_!lXuXPKOb0@@@FvJ0gtd0n|(KKk9jYVr% zI8rNNOZ4}}F14&g6%<<%0T3lk&)p~abEQ2NFOuN?6IP=79ni4_Fx)!{m8-GNOiJdf zsbt3vNZ>ibK0j_1oqK7~HVdIqT&d-6HVhs2r_nFveyk1zKvZa<+#UDB>+KM^zyde# zDsLPDPC-+6^g0Qp+>_ZIHum z&sfoUjzJl4_m)d7*J7oX_Zy|-p&C_PAa=1j_Z#g!-18)NPFiFBoPrY7wSFK-(onm( zb|N^+q?V^!=O2@`vjzsbrDO1DxzuufgwL?hNvUOBZbU*4QiR&!N>o2MZ;goAR@g7R~l-!)GQ=XVNAqBxehtWo{bov4i`4SLUuwEgaqdW&xtsylTyn?r~-}O zDgaVM6LZhFKfYpx$XV9db^tJwL-GSK&NCe~TWTg?_JWz#SQcTGALX5-mMN@im%ehT z<(3HW$uO#8vPG=Y0=El*@L)}Dw3MgUrC2!A23z(idJ-CLjrYnx^%nd}FU*^2gP16( zY*5S~oSjl1>qrX?26u*K$mik_tQkH93a3{-2z4}Wr4`4cCz6X%}MABU;8J(+cyJM z8y5|d*@3qBJ;pL$hrFacFnL}wer+w6TJDBn!*1)1)H07%zdHEeYla z+hgZp#c-?c&w#IgCaN@G{XbNEKEbhWRY4u{kjNx7Y%Z5t?giI@m-I?%Sw$0QHMszY z2F%TkD*xz~1atlEvF`-CnaD~nkVN&Dbyb5Tl;BvS&>^?AV)QxcHDTb$eB1{vE@$*k zYFPtSz+09AAiSBfU`AYhk_R^>nB{Gcqi5CEaeto<`GV*q)NRbd@yHd<-H+*|)Ut-E zfR}6qKzK3Z`!nH6RNuQH#*9f0IDJ{6<4$fVe*PJ#@XN>NB=e`*Vr`5i(%6l8b2DI3 zY$ED5l1nZ3gHx9S1}9#l!KzU@YXK0YO{HE!u0-{n>takDUr>Gb*E)c=Uk1MT03;-} z9J|soKd4(_9rDbF{m{l~uR+P4sb^`;+$8{_8M_w9wXj6>)Cmr_c!TXpash(G(>DWO z)+yMNES_zHRZA68%d`>}wKfqAn#gs?`=V8c19~sDtc5C2`w;>lYMV~A8@Z7DeMN$4 z6YUj{SDA@BUAcJ-2sDhM(|`0w4o7565}LJ^OD*?AOIgp6a8ZlJH_Ar| zfM~hexfA!lE=n-l&mJc)0cI>Dg^NSOs(H(j(5k)RN-b--3RHTW0EkNF;wLe83Haqb z3#7q>yaa9gvA;RxFo?pt&h%&_XV?+!Y0|AXS_Zr0;2=+y0;K}s$2Z(aU!6aeARmMZ7ezKu=i*f{$w zrjAcOdT+{;vC(~d91cEw`tn1SA0pgPBnt?D6v=%2+}x|n_k-pyD7D<8{E-3~rac`J zH)QzL;GH|q-sa~?LN2OWKmbHliwWtC$^rr)MpfOy%c^Pt0T5L!CZsni3kZN1Rdow5 ctEvV552*TFBJwqg%K!iX07*qoM6N<$g8CSFr~m)} literal 0 HcmV?d00001 diff --git a/frogpilot/common/frogpilot_utilities.py b/frogpilot/common/frogpilot_utilities.py index c2de09b44..eae3ee72f 100644 --- a/frogpilot/common/frogpilot_utilities.py +++ b/frogpilot/common/frogpilot_utilities.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import json import math +import numpy as np import os import requests import subprocess @@ -80,6 +81,20 @@ def calculate_distance_to_point(lat1, lon1, lat2, lon2): return EARTH_RADIUS * c +# Credit goes to Pfeiferj! +def calculate_road_curvature(modelData): + orientation_rate = np.array(modelData.orientationRate.z) + timebase = np.array(modelData.orientationRate.t) + velocity = np.array(modelData.velocity.x) + + lateral_acceleration = orientation_rate * velocity + index = np.argmax(np.abs(lateral_acceleration)) + predicted_lateral_acc = float(lateral_acceleration[index]) + time_to_curve = float(timebase[index]) + + return float(predicted_lateral_acc / max(velocity[index], 1)**2), max(time_to_curve, 1) + + def contains_event_type(events, frogpilot_events, *event_types): return any(events.contains(event_type) or frogpilot_events.contains(event_type) for event_type in event_types) diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index f9fca93dd..a9b4a0cc3 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -11,7 +11,8 @@ from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE -from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, THRESHOLD +from openpilot.frogpilot.common.frogpilot_utilities import calculate_road_curvature +from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD from openpilot.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode from openpilot.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcceleration from openpilot.frogpilot.controls.lib.frogpilot_events import FrogPilotEvents @@ -29,11 +30,16 @@ class FrogPilotPlanner: self.frogpilot_following = FrogPilotFollowing(self) self.frogpilot_vcruise = FrogPilotVCruise(self) + self.driving_in_curve = False self.lateral_check = False self.model_stopped = False + self.road_curvature_detected = False self.tracking_lead = False + self.lateral_acceleration = 0 self.model_length = 0 + self.road_curvature = 0 + self.time_to_curve = 0 self.v_cruise = 0 self.gps_position = None @@ -62,6 +68,8 @@ class FrogPilotPlanner: self.frogpilot_cem.experimental_mode = False self.frogpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2) + self.driving_in_curve = abs(self.lateral_acceleration) >= MINIMUM_LATERAL_ACCELERATION + self.frogpilot_events.update(v_cruise, sm, frogpilot_toggles) self.frogpilot_following.update(long_control_active, v_ego, sm, frogpilot_toggles) @@ -74,12 +82,18 @@ class FrogPilotPlanner: } self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position)) + self.lateral_acceleration = v_ego**2 * sm["controlsState"].curvature + self.lateral_check |= sm["carState"].standstill self.model_length = sm["modelV2"].position.x[-1] self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME + self.road_curvature, self.time_to_curve = calculate_road_curvature(sm["modelV2"]) + + self.road_curvature_detected = (1 / abs(self.road_curvature))**0.5 < v_ego > CRUISING_SPEED and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) + if not sm["carState"].standstill: self.tracking_lead = self.update_lead_status() @@ -103,6 +117,10 @@ class FrogPilotPlanner: frogpilotPlan.speedJerk = float(J_EGO_COST * self.frogpilot_following.speed_jerk) frogpilotPlan.tFollow = float(self.frogpilot_following.t_follow) + frogpilotPlan.cscControllingSpeed = self.frogpilot_vcruise.csc_controlling_speed + frogpilotPlan.cscSpeed = float(self.frogpilot_vcruise.csc_target) + frogpilotPlan.cscTraining = self.frogpilot_vcruise.csc.enable_training + frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg() @@ -116,6 +134,8 @@ class FrogPilotPlanner: frogpilotPlan.redLight = self.frogpilot_cem.stop_light_detected + frogpilotPlan.roadCurvature = self.road_curvature + frogpilotPlan.vCruise = float(self.v_cruise) pm.send("frogpilotPlan", frogpilot_plan_send) diff --git a/frogpilot/controls/lib/curve_speed_controller.py b/frogpilot/controls/lib/curve_speed_controller.py new file mode 100644 index 000000000..36febe46e --- /dev/null +++ b/frogpilot/controls/lib/curve_speed_controller.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +import numpy as np + +from openpilot.common.realtime import DT_MDL + +from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, DEFAULT_LATERAL_ACCELERATION, PLANNER_TIME + +CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL +MAX_CURVATURE = 0.1 +MIN_CURVATURE = 0.01 +PERCENTILE = 90 +ROUNDING_PRECISION = 3 +STEP = 0.001 + +class CurveSpeedController: + def __init__(self, FrogPilotVCruise): + self.frogpilot_planner = FrogPilotVCruise.frogpilot_planner + + self.enable_training = False + self.target_set = False + + self.training_timer = 0 + + self.curvature_data = self.frogpilot_planner.params.get("CurvatureData") + + self.calculate_weights() + self.update_lateral_acceleration() + + def calculate_weights(self): + curvatures = np.arange(MIN_CURVATURE, MAX_CURVATURE + STEP, STEP) + mid_point = (MIN_CURVATURE + MAX_CURVATURE) / 2 + + self.curvature_weights = {} + for curvature in curvatures: + distance = abs(curvature - mid_point) / (MAX_CURVATURE - MIN_CURVATURE) + weight = 1.0 + (4.0 * (1 - distance)) + self.curvature_weights[str(round(curvature, ROUNDING_PRECISION))] = weight + + def log_data(self, long_control_active, v_ego, sm): + self.enable_training = v_ego > CRUISING_SPEED + self.enable_training &= not self.frogpilot_planner.tracking_lead + self.enable_training &= not long_control_active + + if self.enable_training: + self.training_timer += DT_MDL + + if self.training_timer >= PLANNER_TIME and self.frogpilot_planner.driving_in_curve and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker): + lateral_acceleration = abs(self.frogpilot_planner.lateral_acceleration) + road_curvature = abs(round(self.frogpilot_planner.road_curvature, ROUNDING_PRECISION)) + + key = str(road_curvature) + if key in self.curvature_data: + data = self.curvature_data[key] + + average = data["average"] + count = data["count"] + + self.curvature_data[key] = { + "average": ((average * count) + lateral_acceleration) / (count + 1), + "count": count + 1 + } + else: + self.curvature_data[key] = { + "average": lateral_acceleration, + "count": 1 + } + else: + self.enable_training = False + + elif self.training_timer >= PLANNER_TIME: + progress = 0.0 + total_weight = 0.0 + + for key in list(self.curvature_weights.keys()): + if key in self.curvature_data: + progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0) * self.curvature_weights[key] + + total_weight += self.curvature_weights[key] + + self.frogpilot_planner.params.put_nonblocking("CalibrationProgress", float(min((progress / total_weight) * 100, 100.0))) + self.frogpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data) + self.update_lateral_acceleration() + + self.training_timer = 0 + + else: + self.training_timer = 0 + + def update_lateral_acceleration(self): + if self.curvature_data: + all_samples = [data["average"] for data in self.curvature_data.values()] + self.lateral_acceleration = float(np.percentile(all_samples, PERCENTILE)) + else: + self.lateral_acceleration = DEFAULT_LATERAL_ACCELERATION + + self.frogpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration) + + def update_target(self, v_ego): + lateral_acceleration = self.lateral_acceleration + if self.frogpilot_planner.frogpilot_weather.weather_id != 0: + lateral_acceleration -= self.lateral_acceleration * self.frogpilot_planner.frogpilot_weather.reduce_lateral_acceleration + + if self.target_set: + csc_speed = (lateral_acceleration / abs(self.frogpilot_planner.road_curvature))**0.5 + decel_rate = (v_ego - csc_speed) / self.frogpilot_planner.time_to_curve + + self.target -= decel_rate * DT_MDL + self.target = np.clip(self.target, CRUISING_SPEED, csc_speed) + else: + self.target_set = True + self.target = v_ego diff --git a/frogpilot/controls/lib/frogpilot_vcruise.py b/frogpilot/controls/lib/frogpilot_vcruise.py index e2acad096..e18be27ea 100644 --- a/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/frogpilot/controls/lib/frogpilot_vcruise.py @@ -2,11 +2,14 @@ from openpilot.common.constants import CV from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED +from openpilot.frogpilot.controls.lib.curve_speed_controller import CurveSpeedController class FrogPilotVCruise: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner + self.csc = CurveSpeedController(self) + def update(self, long_control_active, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles): v_cruise_cluster = max(sm["carState"].vCruiseCluster * CV.KPH_TO_MS, v_cruise) v_cruise_diff = v_cruise_cluster - v_cruise @@ -14,7 +17,22 @@ class FrogPilotVCruise: v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego) v_ego_diff = v_ego_cluster - v_ego - targets = [v_cruise] + # FrogsGoMoo's Curve Speed Controller + if long_control_active and v_ego > CRUISING_SPEED and self.frogpilot_planner.road_curvature_detected and frogpilot_toggles.curve_speed_controller: + self.csc.update_target(v_ego) + + self.csc_controlling_speed = True + + self.csc_target = self.csc.target + else: + self.csc.log_data(long_control_active, v_ego, sm) + + self.csc_controlling_speed = False + self.csc.target_set = False + + self.csc_target = v_cruise + + targets = [self.csc_target, v_cruise] v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets]) return v_cruise diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 7f9e34929..4f11a07b3 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -3,6 +3,9 @@ FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent) : QWidget(parent) { QSize iconSize(img_size / 4, img_size / 4); + curveSpeedIcon = loadPixmap("../../frogpilot/assets/other_images/curve_speed.png", {btn_size, btn_size}); + curveSpeedIconFlipped = curveSpeedIcon.transformed(QTransform().scale(-1, 1)); + loadGif("../../frogpilot/assets/other_images/curve_icon.gif", cemCurveIcon, QSize(widget_size, widget_size), this); loadGif("../../frogpilot/assets/other_images/lead_icon.gif", cemLeadIcon, QSize(widget_size, widget_size), this); loadGif("../../frogpilot/assets/other_images/speed_icon.gif", cemSpeedIcon, QSize(widget_size, widget_size), this); @@ -48,10 +51,22 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil blindspotLeft = carState.getLeftBlindspot(); blindspotRight = carState.getRightBlindspot(); + cscControllingSpeed = frogpilotPlan.getCscControllingSpeed(); + cscSpeed = frogpilotPlan.getCscSpeed(); + cscTraining = frogpilotPlan.getCscTraining(); experimentalMode = selfdriveState.getExperimentalMode(); + roadCurvature = frogpilotPlan.getRoadCurvature(); hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE; hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE; + + if (cscTraining) { + if (!glowTimer.isValid()) { + glowTimer.start(); + } + } else { + glowTimer.invalidate(); + } } void FrogPilotAnnotatedCameraWidget::mousePressEvent(QMouseEvent *mouseEvent) { @@ -72,6 +87,14 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState compassPosition.setX(0); compassPosition.setY(0); } + + if (!(signalStyle == "static" && blinkerLeft) && frogpilot_toggles.value("csc_status").toBool()) { + if (cscTraining) { + paintCurveSpeedControlTraining(p); + } else if (isCruiseSet && cscControllingSpeed) { + paintCurveSpeedControl(p); + } + } } void FrogPilotAnnotatedCameraWidget::paintBlindSpotPath(QPainter &p) { @@ -225,3 +248,61 @@ void FrogPilotAnnotatedCameraWidget::paintCompass(QPainter &p) { p.restore(); } + +void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControl(QPainter &p) { + p.save(); + + QRect curveSpeedRect(QPoint(setSpeedRect.right() + UI_BORDER_SIZE, setSpeedRect.top()), QSize(defaultSize.width() * 1.25, defaultSize.width() * 1.25)); + + QPixmap &curveSpeedImage = roadCurvature < 0 ? curveSpeedIcon : curveSpeedIconFlipped; + QSize curveSpeedSize = curveSpeedImage.size(); + QPoint curveSpeedPoint = QStyle::alignedRect(Qt::LeftToRight, Qt::AlignCenter, curveSpeedSize, curveSpeedRect).topLeft(); + + p.setOpacity(1.0); + + QRect cscRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), 100)); + p.setBrush(blueColor(166)); + p.setFont(InterFont(45, QFont::Bold)); + p.setPen(QPen(blueColor(), 10)); + p.drawRoundedRect(cscRect, 24, 24); + p.setPen(QPen(whiteColor(), 6)); + p.drawText(cscRect.adjusted(20, 0, 0, 0), Qt::AlignVCenter | Qt::AlignLeft, QString::number(std::nearbyint(fmin(speed, cscSpeed * speedConversion))) + speedUnit); + p.drawPixmap(curveSpeedPoint, curveSpeedImage); + + p.restore(); +} + +void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControlTraining(QPainter &p) { + p.save(); + + qreal phase = (glowTimer.elapsed() % 2000) / 2000.0 * 2 * M_PI; + qreal alphaFactor = 0.5 + 0.5 * sin(phase); + + QColor glowColor = blueColor(); + glowColor.setAlphaF(0.3 + 0.7 * alphaFactor); + + int glowWidth = 8 + static_cast(2 * alphaFactor); + + QRect curveSpeedRect(QPoint(setSpeedRect.right() + UI_BORDER_SIZE, setSpeedRect.top()), QSize(defaultSize.width() * 1.25, defaultSize.width() * 1.25)); + + QPixmap &curveSpeedImage = roadCurvature < 0 ? curveSpeedIcon : curveSpeedIconFlipped; + QSize curveSpeedSize = curveSpeedImage.size(); + QPoint curveSpeedPoint = QStyle::alignedRect(Qt::LeftToRight, Qt::AlignCenter, curveSpeedSize, curveSpeedRect).topLeft(); + + p.setOpacity(1.0); + + p.setBrush(blackColor(166)); + p.setPen(QPen(glowColor, glowWidth)); + p.drawRoundedRect(curveSpeedRect, 24, 24); + p.drawPixmap(curveSpeedPoint, curveSpeedImage); + p.setBrush(blackColor(166)); + p.setFont(InterFont(35, QFont::Bold)); + p.setPen(QPen(blackColor(), 10)); + + QRect textRect(curveSpeedRect.topLeft() + QPoint(0, curveSpeedRect.height() + 10), QSize(curveSpeedRect.width(), 50)); + p.drawRoundedRect(textRect, 24, 24); + p.setPen(QPen(whiteColor(), 6)); + p.drawText(textRect.adjusted(20, 0, 0, 0), Qt::AlignVCenter | Qt::AlignLeft, "Training..."); + + p.restore(); +} diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index 424987a8c..f82e4efa7 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h @@ -43,12 +43,18 @@ protected: private: void paintCEMStatus(QPainter &p); void paintCompass(QPainter &p); + void paintCurveSpeedControl(QPainter &p); + void paintCurveSpeedControlTraining(QPainter &p); bool blindspotLeft; bool blindspotRight; + bool cscControllingSpeed; + bool cscTraining; bool experimentalMode; + float cscSpeed; float distanceConversion; + float roadCurvature; float setSpeed; float speedConversion; float speedConversionMetrics; @@ -59,6 +65,11 @@ private: QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } + QElapsedTimer glowTimer; + + QPixmap curveSpeedIcon; + QPixmap curveSpeedIconFlipped; + QPoint cemStatusPosition; QPoint compassPosition; diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index aeb41b6d7..5e88e4dce 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -129,7 +129,8 @@ class LongitudinalPlanner: if mode == 'acc': accel_clip = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) + if not sm['frogpilotPlan'].cscControllingSpeed: + accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) else: accel_clip = [ACCEL_MIN, ACCEL_MAX]