From c51b96879ac6504afe9f86219ea1b524e5a59c48 Mon Sep 17 00:00:00 2001 From: AngusBell97 <124716116+AngusBell97@users.noreply.github.com> Date: Sun, 13 Sep 2026 17:49:18 -0500 Subject: [PATCH] Keep Tesla steering diagnostics in custom cereal Move the fork-specific diagnostics out of the stock CarOutput schema and into StarPilot reserved messaging. Preserve the legacy saturation fallback when custom diagnostics are unavailable or stale. Co-authored-by: AngusBell97 <124716116+AngusBell97@users.noreply.github.com> --- cereal/custom.capnp | 11 +++ cereal/libcereal.a | Bin 645812 -> 645812 bytes opendbc_repo/opendbc/car/car.capnp | 11 --- .../opendbc/car/tesla/carcontroller.py | 21 +++-- .../tests/test_coop_steering_limit_info.py | 71 ++++++++------- selfdrive/car/card.py | 27 +++++- .../car/tests/test_starpilot_car_control.py | 30 +++++++ selfdrive/controls/controlsd.py | 3 +- selfdrive/controls/lib/steering_saturation.py | 11 ++- .../tests/test_steering_saturation.py | 83 ++++++++++-------- .../tests/test_steering_saturation_publish.py | 21 +++-- .../test_tesla_coop_saturation_warning.py | 63 ++++++++----- 12 files changed, 223 insertions(+), 129 deletions(-) create mode 100644 selfdrive/car/tests/test_starpilot_car_control.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 1f14636bdd..908dee7d56 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -14,6 +14,7 @@ using Car = import "car.capnp"; struct StarPilotCarControl @0x81c2f05a394cf4af { hudControl @0 :HUDControl; + steeringLimitInfo @1 :SteeringLimitInfo; struct HUDControl { audibleAlert @0 :AudibleAlert; @@ -49,6 +50,16 @@ struct StarPilotCarControl @0x81c2f05a394cf4af { uwu @22; } } + + struct SteeringLimitInfo { + valid @0 :Bool; + modelLimitErrorDeg @1 :Float32; + resumeLimitErrorDeg @2 :Float32; + cooperativeLimitErrorDeg @3 :Float32; + cooperativeOffsetDeg @4 :Float32; + monoTime @5 :UInt64; + combinedLimitErrorDeg @6 :Float32; + } } struct StarPilotCarParams @0xaedffd8f31e7b55d { diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 9bdac9f8d1c5e5a1ba6251fec0abbeefc2f55976..a242c1f0f8c7a5c14e69223dcdd5d1777eabdf34 100644 GIT binary patch delta 14915 zcmc&*dvsLgwLfQ)nR8Au36lpilMpZh%78TnR163)C~C^(5fCwoU_jKUJVGlN5tx7? z;gJv>WRG5fL9xaMAt;zA2Gic!l`e%FtFA_iG_KM%?L`dtDmPN)-o4MaHy)2=t^U*1 zkCizyzy18)``h1P|AArq4-9KwV?>SD{o23qFZ!=Ej<%!!(%NfA^dD$`^F)#M&-#o0 zvlRb>dT0JKY8IO}Hw0s+`vybFQLp#4`jp7Eu}>#QpyqXBNUR~p_u8x}O~HN(Mit$) z;L0&~&%bMA(N%@>U}=gk2L`oS!{Gngts$}Q9B*vqf@wcsn|IXAfs;Q+j>I(YBkwL) z|Ly`E|2+7Ygn!BS=WTg+f$yVS?^R>2xN(|4` zuk;W+lVbFTThI7XFETYv)A8ThwGFEtAKUM^g}*oAU+;pW8~P>|H>KhCr(xe2-{n>W zzb0wPnir;LB@KY?dyG8jEiscJ@;6_$H%-^HQ7|&g8UTGqbw9ip(EZy^>&g09xb!r# zZ~dDu3|(1CL*YcOX~N_>Pe309drs>EpyqT^GML}^vLSvtDIaQ=d;IWBDt>fc?ny?b zd8t_SYNUIwL?bg`!ZW@B5dWhd_KwHjYvKJ=1KoMjlLxo&N6Y!CNr9G`-}>4;vF0Vo z+kWp$Zh0%&NK|<~$%DDei~`R`d9lFip!Ixzs=8OJfIZ8MTRfe4v9dKm$XjmY>oqWb zxpA4Ndk8FEZp_uo;P7(eM!g;SKZuVGnDZcZNdoE~H15!g;B)#Fhk@1js|!|DBTF^B zLyvMudkAS2F#aLqjtiUb!Un|B=~d538Du|6>+`AK0cd;32u;XVzqG+N*{kIyDM0Xw z9OZV+{R%I{HlR|(cmd2=VT5v;7b>`f$q(l#vC&XR*zc+b>)LF^d?T11el5_YOr^<8Kp`X@~1DWkp2b#H$U zjuE|^>C!e?=myDaM<6|F1mXA#Rv_iyv0(}Ps~3t9%stCOGQ)nAtLQZ!z+$3jvb9m{ z)T^wt;kU4Z=o6VPPTe&?$=7zl5u#6G0yl}awV$F_ehg=ko}=ODr4msOEsyC_;nEr- zlsSvVWjwu`7wcRTY*|=iRCqkKCLCFdI>J05t69g(*uojOxR~(mTH{6!uDEj_F@hs& z4Mi8W0Co?t)i5qLZo+;cT!Xepuvcps7xEv_Lq-0a$Z?nPPq3y$4#U7ljgY6of*rf8 zpdNrJ_%pY`HF?Xh`ox_`fz^*(FL3&TuM1eh9mq#QV~qcpa+m!wNa+*Wi@4>8Y?7={Yq#nMA$}^%1=a zwxjyY9@mPnHUv#521nN!W^OOrmc}c={uYC+)pFQ-wSY=LQ+C z$$&xYv4+X(v9soLWA|miR0MOYxwS;-OLW1maVD%@Zw%^R;Tq98?orEN-+E&J?v;nv z8?*K0Fuc|nkW<4=7gsH$5hIP%8kkdST%s?74ak+!Jw%ZMIt;2spBiuW`ccFCwMMA_ zGM1!AgttN+-anm`1%VBQ30*H+e%!77;)az zN|Rv_QepgNBNfcOmOrf=dx;7uUJvt-8Kag}n^9#)oskBA+-&sA>AFer^f-8_!Mh-> z&bUrr57X;VtQr>8q2X#+haf~Yp+g_mVMWVtV`*McrK08VZJlw6?FC#>Zv-x_n5`IP zAP{4ng6z*q9w3C*!_sUKLq8^)~0-mo&zsu{-+@ynNk|rf?OTk{h$07>zDh_|=D@euX5dh>6^u$~*5IJiSHT-y zcma2LrvrDoEla~Ww-?&Rv;a>q7d}9XxCo=!K$!`vv3;U0vIK(-=fN9YcrmMMb>R{< zb~zm7z095@-Sd{7Y+=fPWDxC(run%3 zX4#x_d}xI!TaC~~5*tW^2VPJP$ti;HUGTCaT2l16D?~iR<9v?PEOd!YRl#c(Wn*hTP-bdf%z`Ie zxExQtvhqC)W99oeR z%^k3E;9C8H{0nRKAw$>e7Y|#f4}VO*tC`VBXhzNk<7x5+Q)Q>X3Lr_b1<&%AlBeoOiD`famc)#uFJuUG8S=l@J! zuv5SLp2}zS``*wOKBq5k)T@^MB%vp#_ybwN@SwawgD;AVC>V9c=rQB3nQ;A&r(<-vn0M!0_uju~3H+|&MFFz8G3 zV$21bQ2tqRAok&waI9ooIRP)c_ob<4yi^rFwDY6--zdG9|3>?mQ2wbNMxKe#h(ayL zzcS}%#LB8tV6tHqzzyKbj}7|FYT3a4QTS-e=~) zP`_0Q6$Kvi$vh)$iX@aR^%S-|;J3ow9KkL`Z93-Z_kKBf%x%5DSfjz3KY6lY&AnFW z$!5&^ANE`M%Fb6)j7wS~y0JB>Wpjp=Sf@u|&N9q<4;e@a^5N+?+$m2M;4QmH^`G`9 zd{W|?qkJs@+rbJ!-{WS!XV5@9i+%zuGl#{{6;&kD*avBG3$sQHi<&ZA1j(EUR{IlFV0+JzS{b`BnDMd9$9tzXiSbf}XUX2Lwcx}i zMHT&U{j@N9Y_9^}z7Labbm<7(d z@Nt+pBL41SicrLQvTZy`i?9Oe&tK*$eh>4h3S&2&U_vtHYo5jy2@h8a1I+jpE~jM3 z$G>@D)z-7akr9etZa(g?@g%K~cTsVmAM^Xa=YXbN#dx~Hqvx7@3OiPQi82sj%NN;r zlGf@n@KKI(R2;`1u*X>6{{Jk_*6C)PlHuV7{>xLSkZHQMP-grKqqT@ z*v6Bzc6Lx|dQVHlYntLL=TUi(2sp({X=lU2V^6!^#=Y8D z=98|8F<#=rS708QYFx-QYGC>Z#&5viSpV-P(+nm|Wk#u4F^2A>sC_1JYg!2tPU3hX zd=~TP+f+>pGXHWHPN_)Z*XAfjTor8l+Iu!3N$X<8VOF$~1-7vwSuS=nzT1UA$PSgw zQwmDB$)_>?D)URlUbdiMyXDJin6LQrng0>o6v)vxVaj$~ESv9C4=scFkGk;wcqc-7$}5$g1kdqzc#Sxw_@n0r z$mdKr$%r@8grPo@Cfu{C)g3R7iNk_i(KCYrHyzm%xr4 zxJoCMD8YPoL~fGK!r>j*V9DH$GJtR8U8H@f66<98NE=VmkEv{J#^+hqy!H?y8X#)0u}g{Nvl$Qej?aj
    t-0B11WdL+2b1iuk~6CQ@cyD)4T8n*&huEAkLSiXz5k?}C&(jI3>H|&dB zS)Py&7C&oY*5_Hv%x&cM6kENx(vjYX2j(Kvs07nR&wt~_VB6&i3;r(SktC>p-U^OL zxafOv3njTm#>L(2Asrav=nzw;xFQEmr5jYC0bqhQ5=iJ-@&-(zYaGs!b^D5 z6Fu~L0O92f2>&~iBWO?*SJNHo=U6+72$uD z@hXMGke^}3u*b#pEe?3wNybI#D4g=PQe0-8O6f%w+iAvyf0OEKt;fZ`fN@SC!fWUR zWbeeB&uF$RNkGx`Mi2&JbB%ohcM!7VzMri_3$_P1h@76?&3JV`D5mC?WSKO__4tM4 zOsoMFvG*d^sD^RjU(0x{i@z4Ti}c`!U4(xgIAs@coUJ z30Kh7a5?HAZGlWEXtaWpYH{J9B3d7?4JBy}jEmPdhLxNoZ-hUe@%${PZ?pzkayB(# z&WlzkHhH576~Fc-d2`#|$ve6|Yc)JT93vDl1bm{(=m#hIP z6+AriJQICyIE? ziY4JMb>P&3B98|ky9mFW)+=0ivKkHeT8jN*!FRj22!@A4{T|$v@p*`zJ{?Z(u?nnn zr*&wMS3j@Ik z62XXWInQ^ybO_$(!f$}-bG&LY%E><8gkrof9w|YPsFo9c1+UF+yc)*u!?3w^4qoA! zuUdXj!nB(qSE42nkRyH<8p39ab~_Y;-JI&9$AQNkc%K8etHUV+d8vTxPlUsi~o8*q{`aK4rBl*6#ZX;KkMB~)|#=EmJGB}j_F`SQUw4JK_dQbzW;bV%Bv z?>u+__Pl0=(wwgu0PHznhKi|*=duzU8j5Fv%d38Z|AN{J3%=@CydP&)t9ltADxszD zN!p^%rB`s-qJ&@48P4|z$UjME6!BI^{LULZ4q3nJ8p(51I|N9P;0-RE^B-Emg-iOQ z)q(G#nsbrZS5;skMh^H#rOzQih6OL;_>}~nvpLm=1jW)4l1Gsj9^H7W-@cm8lsH-F z=3+SZB(5!zZz)Q%-z6AT$?>l6N#3RUJa}swOvUT_Oo@|44%0G6wizL7iE_Ir9abaT zgi1WJP!TrC+EkwhZ*bvfq3v}mlp@)j)^v5N_vP!jI!h`CUxdUr3POb(u!t!k2OKyJ zPs#LzVSEd&Gd*{D{khZ=lF@NqhVRv&c!4))-xqRbN2_t@BRRq2E?g2mjV^oy@A>Tx zd>`tOB5}Xe12aP-Y?{2oN(85XWH$)k3sVl7A=_V+vr9h5|3s$xi<*c7r+P?=smO&( zimA+jzot~%pK|UHe=+r>GOi5{0Wu&tsYVwrIjK$ue!yWs4k}_ma#aC7z_{^9Mv0OZ z0w=MwGKWBm!$6G#Cr7&;fwOpP@4;um(?(F=1|!=rE@+HnT<|5{&<7>EW%8kh3?0zm z*xfu-gdTCwsp*Bk(1DXd$%d7-Xq_^8ez=e%YoeuPQU0;eOP4p2D(76 z)i2wmj~sP*y?*8BtDexujD1`mH@-o?rewlX`gPmz`c}WObUR+4@6@ME-K9ULKd--_ zzo@^g-#lxNUViIsd-dDr?9=b4m@w}({mujWT`l@OZ|L{lcmF|sQJcP`YU$f}g`OVB z3gr&WAA0eKg2F3`uExvxYbW0DpR|5Fpg{cvc8MEp<}DMtWPwm%leMS?_bt;9#(Su)n$E0BX^PvEd2RX`?CK3 E0SLj(2><{9 delta 14995 zcmc&)dw5jUwLfQ)%$YL@Ve+0lhyw~l9&rc=0VRfqHKl+Nd_=$)5ilZ;Av}xu=2 z@A^}#_RsnR|5=LvPQ6qAk)FYCKZ(nLaSu2LK=HY_xZ~U1c@z829Wip$JpZWq3kv3q zUNCZQG*NrCk8*U z_=#(If1dLfS#gCUMvfT`6HXd;#8lj+!z-6u16#fhI(wawrll_Ue52E&cSQCt&4~1@ zij9og{GhApPal0Yy<^l-m~+Kd4Tb$3o=9OR(DL~vXJex7hW#%)Gj;c_4QBtAQ!hLB z&DC|7@PCf%K4Y}&*Q1gCUuM9bzc_t`35e?W$6Ax{c_)6>uB}_MV_e^hY54yZ{PfH_ zx4Cz6QDY|lzZ*YrKHl`g#lJZH<}iGW(PFi@NbQ%|`hdvZFSFsnubkdU$Ct5s0sL{d zGaGu(=?Uve$mS;;Bi8ua_DwJp^$9;}Hk+i0Pi`+`8e!GUE=t4Sv7Ocw8@od#muN6;@W^Q!o6Y3R&u*-*UWF z!~Eq)3&F1C#^dge`Di>|Atzj2Zlnjj@=^QBBYcX}oG}teCwu_T1A*r#_(JqX;H8Ts zV`;3sXAxdN{E4JzBH_h1;sdk_oJc}ZG$1+sI4I&OBz!t7USVWpb}W-{F_B-$kzx!h?DfP8rUxAjYLl-^%*2z8iG?u&-BGhf{W;{q)7QTxGb9g zcM^B7cpp+<3%RRMyn)0^SsdSG;rwZswhG1P(9qNq*@)MbI@OaNne$8A9IobI5(pNQd>Wn;!QMuww=2cEWT7 zvvNrYCz*BqG)vN>olqm_1w=0(`Y}@4bPgH?eIC*2rp?cm@(t(Vf}k%X0(FV@QeR0A zUx38tu}i}^d&QL}Z(0t~m%#Yvjf~_~B+m0`XTL~CZF}Ql^V+WTyW`KZS4@&aABt4$-V;@q3Wpq9?#~q-VO6=1XxM5%gYI6GZ<5%CfQ+ z-z-T!g5<1fYMpzp7^j7$y$@Uvt@kSQ#mFe=S|TKdu=Q$sqgnn(xHwDo<3qwZ91Od2 zZ_~(D_k|@wJfzT@sqcDWFN&vClkcp3HjZ@?jCjsSfpZ}otkhZ@to!lB5>2|2;VSBc zX7M)8rA`hdLw+s#p;0ffrq9uC%g+=juQhVrp=+-LTxl?IWKlD2)<DMHrI5&X@(Rx3H;~)?rhD$x@=-#w=_Kz>n7%ll0Y~ zuQ#$WUg@_Ur+XLNxgNn9cn(2F_Y|0Z$V|s5=E!<9KfDeNy0{*@v~8-?SsVlVHer`~ z%igxn{2p! z40x+xv#y1!atjGwbD9Z$+FMwloEj)ra1LzCY`A?Mg>AU#$GlZ?d^q40`uA!<9yCp> zw+ZaK4r>iI+^(qAhKu-=6}Q=N!CyrVbSStLT8meu+q7nU&`aAUdmgq{FSPt7c(H1lidDYnrY;r17eY`34EP z9g%JMoL1{58^6%Q$Cq{+F5846baqB(qjc0o@doQ-+^67t-0>^8i`M&+K2|3OIlf_i zd<+UJa*Rh6Jc0BzDL5aO+HLqT>4No5aTENy*>pe9OukX9#I|?|_bIrK+Vm@U0sEE+ zS9W01fMYM#$7isxBFA`C!6&nC3eJbMb{lS=puG~mzFhiFk%JV?u|bUc6ug2RRPbtc z(1y3zIAQ+gG8_s_^dibvT&#(fIT z2hI|Y^*T-AIF{u)JIHQDj`661^8vNpV=WEIy$T%dfC@!^JD@rbwq%!TI3Zkz_41O6tLdE7UnlYFp$rom6ahYIci;g6dus}x71-d0^JFlc zAyhn1!TDrfnqsZjSs{fUz9995Q>^#6pw32blqBoB-i9Bu;n5UpmIfPabo*=4YQsf` zaC_}GTnucOo;z%~;9o)m+oj-~GU$~!#OyGVf^kmsx%lcniG4$QQu6B5w3>`HnOWJv zejD^#eEq+#-+J2?ebC^re*0#9=&<1t{SN<#o%+a8FYBX0FWjY%yQ@wgUv$q7eZpRS z(mws(@7#Al|L&CUHR{u*-+x$tV8)yJgAX0kA1Q0rXU#sOKUUtNS3L1U{mIw$`ETh@ zy{bRGaLhAr=!;M3OB?iMNA%?@R=%dkx)VGp>E4{&0R!`fX>of%1s zA6H5c0?z!K`h&$g_iuO?9XBdq+NXFD=s0eqx*Pl9i-_0T{ou-RqsEb!3QL-e(e5ro z;@&i96wj5Z7Mte!EiMAaXh+}+IQuKJ$Wi|V=Cacx9e?q_))R);?9GvKC*jBmV@z88 z7gDT*XFWbX3s3yYOxcuh(r8II>6$Y-?#{9PF$GrD!%=(@_FZxff)6gaJdx@(-iW8Y zOw>N!c<{1I7yq=JyW*-!!vCDR%FO%|2h5LS;H4oZX4A*vk-Rd`90%_>O*fp2HC+Qm zI3Vg7Mfd0ZeD&$OP6^Y*(X8a5$&;2YSy8og5k5o~|7kExbvbh2aGY6!nq$Eeb;h<# zb(-F|mgc@@bc5>&%>CS$x1W<^fp zqeP!Wa%}TKEFcNFcG^KI=suJ1a^f$d`wN8cQSeP@h|rVQ-y#IHdg`+=MEIBlxYHgd zoYuZ;dFqu}Qc>FgDZn|8X%>!oI_f;`v~MW&`#aJv9mA+{ASt@2!S;GH{odYNB?qTq zzQCnTSo{eI(%@)qxP1xSHBb_`@=-5v_;A0O5-cH>OE74wnwW-#tyGf z1BnMUpF%iJ`EJiN*+3o9pCo(`J##01pYVJIe-`J4@I89J)Kg3Xg9#sP@#9&{C+XV# zaDF!?BZiZ~U5`kdOKWlDU@7VGlGq7635a%9QTxbdUFQj3ui#DCq~Q0{D_Tq)9G18P zYognJU}6IIF{#H#^mTZ27jnDdaups$>*$TGruQVDruhN5jMBkIY89Qet}3^bcFmQ~ zJlh8l{tofeO`Agay9z!XV*p`A;geENl-~2J^qQZN{6US!e-H_rB}I$__McGH`IJ-? zmN#t(uJ;13sJs!Mihc^bTfrk3vI{)$w4EQ_zC`On@6(XD7ZY!xg_42i8yABv!1%pp z#_)?|AJ5O7_^JrK(M3|MgZg=)z~PD4%#^3Ih~-lf<-VRs_?HS^L^C1~kScj*UqM09 zl!Ka}38tkJzZNW4Xlws3C+hp&s~7v z+h~+}txXxrML2QAh2C3N18vrIq{mNq2jP?{ir1JhBS#d_AU10H9Q7I$Ma543yy2N4 zq6$%J2$KdL)^~7v2;55@Sxxlc63!bJ^Jkzdg1=6Ov#30aCos$PP&-i_t|C3?r4D8J zp}3U``FaJ9B|J(v>+6d>V%et*KsWAs0{0Ql{J+56%F-`!i0n6$lJjU?V5Y`QSh62` z=7asFYod>cY@v@d^J2pIzj>wRLfSW38AiPOP+7qzMCbU8iQ}{(DPv9Nm^n@4lr{b0*@KObTNa_h{9ZCatkw6Iv zutDEa1MLdF4Oa%yKnXE2|5)PhCV&kZg?oqKue8#?P<7B8Fm?gys8o0>NJp2V+p58# zjtV$)(99_4vQ%CF*}`woCDBAT;oS6B)O3X#PQ8H1N=^(j{Ue%fU2Z7HF`dh~UZyYg zO1;7?aj~l1t2hTLXdPgE-{4zdbrIpL?-|@*EPV?9 zWWu`@{?oYa2>uG}Uc0?}39pnm%zVvED(F&}L`))ty9s9_KBHH)A`$Yjlad1zWH9}w z39$V&e7oK~V7eSViEsfQrc|PZb_?!BSA}x5_WC>Wgp*&~BR?V>>)BTi;=04hET-?q z1>e$#Zx!|^jC#)SI98>|3i;vJO)Artp$&p9oCIfI7i%MyX~(U=wL*X{T7B7(5U#S8 zPQtmZM+vXTvhEvZZW^Eam@ZN?%V0+>7B_ohpt08YP8zLhnrL5Kf~5+~wMMqd>qQ-G zYru^)YSAT!c+r^L1UnwWA0c{QbtORO8)m}Xc7>7NVy(-DFQfm(jaPC`9Pz=>M=QUI z7wS-sMQBHb4%_iQt4`tLVhf+Sg#5F%_8M-&-)h4}x76#9|0dRdN5j2unpD1C{if*& zHqeyGqt0Uk8824wS=4x`f>UU#@!qZ)%3BfZr&HDNz&D4?4A}Y>4jLbL{j{sHPmGr+_$QwzDP<|K}l1n7qWuptZD;lgiZ05W9`0S5^ z+>MyUK7H8qI0^^BM@V$^XfPdrnvljv;}Grf!rFB(;)t1@#i!#o8%gM|7zT|f>gH3j zhE;ylKa0Zvc9>7gh4g+~s>xHXPD9cmR|@_n zVQizBZSsM-4Td+G86(J01`Zc(Ecrj)xSSkK^_q9N7KfkcRc(G5vl^ai^k)&+k17F3;~M zLVEx!dRTNlUS1dEWAA5ijKuV7=X#e_@y;r<$?ue+)^`QcHyi0g0yerZf#pLsTr^qg zf(1=x?m+cN3w-{!|7hVQ_zC{*p-8CXG(e9{ogH^kJjLekB=`JQ!9F9XkU47*xCtIs z@ZF@Y$%fl|fx|p*p3@FJ#6M9bxab(j_yHac1?L2X-wju{U{SL|p>v`la1%VN;GDov ze_+4~j3yiZZfk($SC_;mU7OIH+32wehyh{TMS&$dNEr{!8xJ+dFtD`$@_@G^(G&-; zEXRqDfTE9aktY(D;NK_zwZ1v9Wj&lI>5=>?*BoJjKLLucyp_xmFAH{8LO%XH!Oa8~xphW{`Lr~?sgG3?oy##VvvFRghhkn8 zcTud%_ywL&3eE`}f1*{*%c@Z5oXDxP=oz=)glsROiNsMw8frr{>KOBPhf_l&_+INn*bcwvl!=v0yp8eSI-aG^n`8v zVuG`tdWp+2zvu#1tX9=I2(umH5=y1jRvrGkOpn^=qWhU&t;2JIOs&JOg`+22DZ!IgBlJbbDee$Xo~fmH z#`6?@PO8~U@vS!fYAGI*Z8DDLJJ&E&W+<<_p<5YjSV#h4cZz+XfC=ujk!9 zWSu^2qn>|d~dyeU&(HL%AD`*(f8?5 z{eb?e{<=P6W`q9F!;c)&ADwkrpYvFGqh8UZ&poEkYt|P$Re4fh*s3pDyyP7$Vkdf1 zGqU>izjg4?;Un%Wyep#LGhxzqzB?7`;-wEfST_6dC!U=D^fRK0y(BVc$>5f4KQd>U z5p&_~EgkQf2a=)mQ&+52UN+DCr*y9hE+037x(60F8)MuJ%@VCnmc?c9i+6D~yeNyy gy$57**>~{9; dict[str, bool | float | int]: + return { + "valid": self.steering_limit_info_valid, + "modelLimitErrorDeg": self.model_limit_error_deg, + "resumeLimitErrorDeg": self.resume_limit_error_deg, + "cooperativeLimitErrorDeg": self.cooperative_limit_error_deg, + "cooperativeOffsetDeg": self.cooperative_offset_deg, + "monoTime": self.steering_limit_mono_time, + "combinedLimitErrorDeg": self.combined_limit_error_deg, + } def update(self, CC, CS, now_nanos, starpilot_toggles): if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: @@ -125,7 +126,6 @@ class CarController(CarControllerBase): # TODO: HUD control new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = self.apply_angle_command_last - self._write_steering_limit_info(new_actuators) self.frame += 1 return new_actuators, can_sends @@ -168,7 +168,6 @@ class CarController(CarControllerBase): new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = self.apply_angle_last - self._write_steering_limit_info(new_actuators) self.frame += 1 return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py b/opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py index 05ea35da2d..3515401f50 100644 --- a/opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py +++ b/opendbc_repo/opendbc/car/tesla/tests/test_coop_steering_limit_info.py @@ -5,11 +5,13 @@ from types import SimpleNamespace import pytest +import cereal.messaging as messaging + from cereal import car from opendbc.car import gen_empty_fingerprint from opendbc.car.tesla.carcontroller import CarController from opendbc.car.tesla.interface import CarInterface -from opendbc.car.tesla.values import CAR, DBC, TeslaSafetyFlags +from opendbc.car.tesla.values import CAR, DBC BASELINE_SHA = "a80064be4fdf4b8765a5e9dec44d8a5c266f48a8" @@ -62,21 +64,25 @@ def run_frame(controller, requested_angle=0.0, torque=0.0, speed=15.0, measured_ ) +def get_limit_info(controller): + return SimpleNamespace(**controller.get_steering_limit_info()) + + def legacy_actuator_dict(actuators): - values = actuators.to_dict() - values.pop("steeringLimitInfo", None) - return values + return actuators.to_dict() def test_steering_limit_info_defaults_to_invalid(): - actuators = car.CarControl.Actuators.new_message() + controller = make_controller() - assert not actuators.steeringLimitInfo.valid + info = get_limit_info(controller) + assert not info.valid + assert info.monoTime == 0 -def test_steering_limit_info_round_trips_through_car_output(): - output = car.CarOutput.new_message() - info = output.actuatorsOutput.steeringLimitInfo +def test_steering_limit_info_round_trips_through_custom_message(): + message = messaging.new_message("starpilotCarControl", valid=True) + info = message.starpilotCarControl.steeringLimitInfo info.valid = True info.modelLimitErrorDeg = 1.25 info.resumeLimitErrorDeg = 0.5 @@ -85,25 +91,25 @@ def test_steering_limit_info_round_trips_through_car_output(): info.monoTime = 1_234_567_890 info.combinedLimitErrorDeg = 3.75 - with car.CarOutput.from_bytes(output.to_bytes()) as restored: - restored_info = restored.actuatorsOutput.steeringLimitInfo - assert restored_info.valid - assert restored_info.modelLimitErrorDeg == 1.25 - assert restored_info.resumeLimitErrorDeg == 0.5 - assert restored_info.cooperativeLimitErrorDeg == 2.0 - assert restored_info.cooperativeOffsetDeg == -4.5 - assert restored_info.monoTime == 1_234_567_890 - assert restored_info.combinedLimitErrorDeg == 3.75 + restored = messaging.log_from_bytes(message.to_bytes()) + restored_info = restored.starpilotCarControl.steeringLimitInfo + assert restored_info.valid + assert restored_info.modelLimitErrorDeg == 1.25 + assert restored_info.resumeLimitErrorDeg == 0.5 + assert restored_info.cooperativeLimitErrorDeg == 2.0 + assert restored_info.cooperativeOffsetDeg == -4.5 + assert restored_info.monoTime == 1_234_567_890 + assert restored_info.combinedLimitErrorDeg == 3.75 -def test_active_cooperative_controller_publishes_r_n_a_t_f_diagnostics(): +def test_active_cooperative_controller_reports_diagnostics(): controller = make_controller() requested_angle = 20.0 now_nanos = 1_234_567_890 actuators, _ = run_frame(controller, requested_angle, torque=0.9, measured_angle=0.0, now_nanos=now_nanos) - info = actuators.steeringLimitInfo + info = get_limit_info(controller) assert info.valid assert info.monoTime == now_nanos assert info.modelLimitErrorDeg == pytest.approx(abs(requested_angle - controller.apply_angle_last), abs=1e-5) @@ -129,7 +135,7 @@ def test_cooperative_offset_alone_does_not_become_limiter_error(): actuators, _ = run_frame(controller, torque=0.9, now_nanos=1_000_000_000 + frame * 10_000_000) assert actuators is not None - info = actuators.steeringLimitInfo + info = get_limit_info(controller) assert info.valid assert info.cooperativeOffsetDeg > 2.5 assert info.modelLimitErrorDeg < 2.5 @@ -145,7 +151,7 @@ def test_combined_error_keeps_two_same_direction_small_limits_visible(): actuators, _ = run_frame(controller, -2.5, torque=-1.5, speed=12.5, measured_angle=0.0) - info = actuators.steeringLimitInfo + info = get_limit_info(controller) assert info.modelLimitErrorDeg == pytest.approx(1.5045133, abs=1e-5) assert info.resumeLimitErrorDeg == pytest.approx(0.0, abs=1e-5) assert info.cooperativeLimitErrorDeg == pytest.approx(1.5045133, abs=1e-5) @@ -158,26 +164,26 @@ def test_combined_error_keeps_two_same_direction_small_limits_visible(): def test_intervening_100hz_frame_retains_matching_50hz_sample(): controller = make_controller() first, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_000_000_000) - first_info = first.steeringLimitInfo.to_dict() + first_info = controller.get_steering_limit_info() second, _ = run_frame(controller, -40.0, torque=-1.5, now_nanos=1_010_000_000) - assert second.steeringLimitInfo.to_dict() == first_info - assert second.steeringLimitInfo.monoTime == 1_000_000_000 + assert controller.get_steering_limit_info() == first_info + assert controller.get_steering_limit_info()["monoTime"] == 1_000_000_000 def test_inactive_interval_clears_sample_until_next_steering_update(): controller = make_controller() active, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_000_000_000) - assert active.steeringLimitInfo.valid + assert get_limit_info(controller).valid inactive, _ = run_frame(controller, 8.0, torque=0.9, lat_active=False, now_nanos=1_010_000_000) - assert not inactive.steeringLimitInfo.valid - assert inactive.steeringLimitInfo.monoTime == 0 + assert not get_limit_info(controller).valid + assert get_limit_info(controller).monoTime == 0 resumed, _ = run_frame(controller, 8.0, torque=0.9, now_nanos=1_020_000_000) - assert resumed.steeringLimitInfo.valid - assert resumed.steeringLimitInfo.monoTime == 1_020_000_000 + assert get_limit_info(controller).valid + assert get_limit_info(controller).monoTime == 1_020_000_000 @pytest.mark.parametrize(("candidate", "cooperative", "steering_disengage"), ( @@ -190,8 +196,9 @@ def test_diagnostics_invalid_when_not_in_supported_active_path(candidate, cooper actuators, _ = run_frame(controller, torque=1.5, steering_disengage=steering_disengage) - assert not actuators.steeringLimitInfo.valid - assert actuators.steeringLimitInfo.monoTime == 0 + info = get_limit_info(controller) + assert not info.valid + assert info.monoTime == 0 def test_actual_actuators_and_steering_can_match_pinned_baseline_fixture(): diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 2e83ce373d..9e2d1a4e8d 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -43,6 +43,22 @@ REDNECK_DECREASE_LOOKAHEAD_POINTS = 10 SLC_SOURCE_NONE = "None" EventName = log.OnroadEvent.EventName + +def _build_starpilot_car_control(steering_limit_info: dict[str, bool | float | int] | None, valid: bool): + message = messaging.new_message('starpilotCarControl') + message.valid = valid + if steering_limit_info is not None: + info = message.starpilotCarControl.steeringLimitInfo + info.valid = bool(steering_limit_info.get("valid", False)) + info.modelLimitErrorDeg = float(steering_limit_info.get("modelLimitErrorDeg", 0.0)) + info.resumeLimitErrorDeg = float(steering_limit_info.get("resumeLimitErrorDeg", 0.0)) + info.cooperativeLimitErrorDeg = float(steering_limit_info.get("cooperativeLimitErrorDeg", 0.0)) + info.cooperativeOffsetDeg = float(steering_limit_info.get("cooperativeOffsetDeg", 0.0)) + info.monoTime = int(steering_limit_info.get("monoTime", 0)) + info.combinedLimitErrorDeg = float(steering_limit_info.get("combinedLimitErrorDeg", 0.0)) + return message + + # forward carlog.addHandler(ForwardingHandler(cloudlog)) @@ -86,7 +102,7 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks', 'starpilotCarControl']) self.gps_pm = None self.can_rcv_cum_timeout_counter = 0 @@ -99,6 +115,7 @@ class Car: self.initialized_prev = False self.last_actuators_output = structs.CarControl.Actuators() + self.last_steering_limit_info: dict[str, bool | float | int] | None = None self.params = Params() self.params_memory = Params(memory=True) @@ -396,6 +413,12 @@ class Car: co_send.carOutput.actuatorsOutput = self.last_actuators_output self.pm.send('carOutput', co_send) + starpilot_control_send = _build_starpilot_car_control( + self.last_steering_limit_info, + CS.canValid and self.sm.all_checks(['carControl']), + ) + self.pm.send('starpilotCarControl', starpilot_control_send) + # kick off controlsd step while we actuate the latest carControl packet cs_send = messaging.new_message('carState') cs_send.valid = CS.canValid @@ -450,6 +473,8 @@ class Car: self.CI.CC.update_live_params(live_params.roll, live_params.angleOffsetDeg, live_params.stiffnessFactor, live_params.steerRatio) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.starpilot_toggles) + get_steering_limit_info = getattr(self.CI.CC, "get_steering_limit_info", None) + self.last_steering_limit_info = get_steering_limit_info() if get_steering_limit_info is not None else None self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC diff --git a/selfdrive/car/tests/test_starpilot_car_control.py b/selfdrive/car/tests/test_starpilot_car_control.py new file mode 100644 index 0000000000..9bf54b12bc --- /dev/null +++ b/selfdrive/car/tests/test_starpilot_car_control.py @@ -0,0 +1,30 @@ +import cereal.messaging as messaging + +from cereal import car +from openpilot.selfdrive.car.card import _build_starpilot_car_control + + +def test_steering_diagnostics_use_reserved_custom_message(): + values = { + "valid": True, + "modelLimitErrorDeg": 1.25, + "resumeLimitErrorDeg": 0.5, + "cooperativeLimitErrorDeg": 2.0, + "cooperativeOffsetDeg": -4.5, + "monoTime": 1_234_567_890, + "combinedLimitErrorDeg": 3.75, + } + + message = _build_starpilot_car_control(values, True) + restored = messaging.log_from_bytes(message.to_bytes()) + info = restored.starpilotCarControl.steeringLimitInfo + + assert restored.valid + for field, value in values.items(): + assert getattr(info, field) == value + + +def test_stock_car_output_has_no_fork_specific_fields(): + actuators = car.CarOutput.new_message().actuatorsOutput + + assert "steeringLimitInfo" not in actuators.to_dict() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 94363186d4..0fdf7c97cb 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -390,6 +390,7 @@ class Controls: self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', + 'starpilotCarControl', 'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'radarState'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState', 'starpilotLateralState']) @@ -884,7 +885,7 @@ class Controls: ) now_nanos = self.sm.logMonoTime['selfdriveState'] if REPLAY else time.monotonic_ns() self.steer_limited_by_safety = is_angle_steering_limited( - self.CP, CC.actuators.steeringAngleDeg, CO, output_healthy, now_nanos, + self.CP, CC.actuators.steeringAngleDeg, CO, self.sm['starpilotCarControl'], output_healthy, now_nanos, ) else: self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 diff --git a/selfdrive/controls/lib/steering_saturation.py b/selfdrive/controls/lib/steering_saturation.py index 827527a3a4..b2da8b04cf 100644 --- a/selfdrive/controls/lib/steering_saturation.py +++ b/selfdrive/controls/lib/steering_saturation.py @@ -3,8 +3,7 @@ import math from opendbc.car.tesla.values import CAR, TeslaSafetyFlags -# TODO This is speed dependent -STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees +STEER_ANGLE_SATURATION_THRESHOLD = 2.5 _MAX_STEERING_LIMIT_INFO_AGE_NANOS = 100_000_000 @@ -13,7 +12,8 @@ def _legacy_angle_steering_limited(requested_angle: float, car_output) -> bool: STEER_ANGLE_SATURATION_THRESHOLD -def is_angle_steering_limited(CP, requested_angle: float, car_output, output_healthy: bool, now_nanos: int) -> bool: +def is_angle_steering_limited(CP, requested_angle: float, car_output, starpilot_car_control, + output_healthy: bool, now_nanos: int) -> bool: legacy_limited = _legacy_angle_steering_limited(requested_angle, car_output) try: @@ -24,7 +24,10 @@ def is_angle_steering_limited(CP, requested_angle: float, car_output, output_hea if CP.carFingerprint != CAR.TESLA_MODEL_3 or not cooperative_enabled or not output_healthy: return legacy_limited - info = car_output.actuatorsOutput.steeringLimitInfo + if not starpilot_car_control.valid: + return legacy_limited + + info = starpilot_car_control.starpilotCarControl.steeringLimitInfo errors = ( info.modelLimitErrorDeg, info.resumeLimitErrorDeg, diff --git a/selfdrive/controls/tests/test_steering_saturation.py b/selfdrive/controls/tests/test_steering_saturation.py index 520b74e614..c7425c79b8 100644 --- a/selfdrive/controls/tests/test_steering_saturation.py +++ b/selfdrive/controls/tests/test_steering_saturation.py @@ -1,5 +1,6 @@ import math +import cereal.messaging as messaging import pytest from cereal import car @@ -33,39 +34,44 @@ def make_case(candidate=CAR.TESLA_MODEL_3, cooperative_enabled=True): output = car.CarOutput.new_message() output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE - info = output.actuatorsOutput.steeringLimitInfo + + diagnostics = messaging.new_message("starpilotCarControl", valid=True) + info = diagnostics.starpilotCarControl.steeringLimitInfo info.valid = True info.monoTime = SAMPLE_TIME_NANOS info.cooperativeOffsetDeg = 5.5 - return cp, output + return cp, output, diagnostics -def detect(cp, output, requested_angle=REQUESTED_ANGLE, output_healthy=True, now_nanos=FRESH_TIME_NANOS): - return is_angle_steering_limited(cp.as_reader(), requested_angle, output.as_reader(), output_healthy, now_nanos) +def detect(cp, output, diagnostics, requested_angle=REQUESTED_ANGLE, + output_healthy=True, now_nanos=FRESH_TIME_NANOS): + return is_angle_steering_limited( + cp.as_reader(), requested_angle, output.as_reader(), diagnostics.as_reader(), output_healthy, now_nanos, + ) def test_light_offset_does_not_count_as_limiting(): - cp, output = make_case() + cp, output, diagnostics = make_case() - assert not detect(cp, output) + assert not detect(cp, output, diagnostics) @pytest.mark.parametrize("field", ERROR_FIELDS) def test_each_genuine_limit_is_visible_during_cooperation(field): - cp, output = make_case() - setattr(output.actuatorsOutput.steeringLimitInfo, field, 3.0) + cp, output, diagnostics = make_case() + setattr(diagnostics.starpilotCarControl.steeringLimitInfo, field, 3.0) - assert detect(cp, output) + assert detect(cp, output, diagnostics) def test_combined_small_limits_remain_visible(): - cp, output = make_case() - info = output.actuatorsOutput.steeringLimitInfo + cp, output, diagnostics = make_case() + info = diagnostics.starpilotCarControl.steeringLimitInfo info.modelLimitErrorDeg = 1.5 info.cooperativeLimitErrorDeg = 1.5 info.combinedLimitErrorDeg = 3.0 - assert detect(cp, output) + assert detect(cp, output, diagnostics) @pytest.mark.parametrize(("error", "expected"), ( @@ -73,24 +79,24 @@ def test_combined_small_limits_remain_visible(): (NEXT_FLOAT32_AFTER_2_5, True), )) def test_diagnostic_threshold_is_strictly_greater(error, expected): - cp, output = make_case() - output.actuatorsOutput.steeringLimitInfo.modelLimitErrorDeg = error + cp, output, diagnostics = make_case() + diagnostics.starpilotCarControl.steeringLimitInfo.modelLimitErrorDeg = error - assert detect(cp, output) is expected + assert detect(cp, output, diagnostics) is expected @pytest.mark.parametrize("age_nanos", (0, MAX_DIAGNOSTIC_AGE_NANOS)) def test_diagnostic_age_bounds_are_inclusive(age_nanos): - cp, output = make_case() + cp, output, diagnostics = make_case() - assert not detect(cp, output, now_nanos=SAMPLE_TIME_NANOS + age_nanos) + assert not detect(cp, output, diagnostics, now_nanos=SAMPLE_TIME_NANOS + age_nanos) def test_signed_cooperative_offset_is_valid_data(): - cp, output = make_case() - output.actuatorsOutput.steeringLimitInfo.cooperativeOffsetDeg = -5.5 + cp, output, diagnostics = make_case() + diagnostics.starpilotCarControl.steeringLimitInfo.cooperativeOffsetDeg = -5.5 - assert not detect(cp, output) + assert not detect(cp, output, diagnostics) @pytest.mark.parametrize("scenario", ( @@ -104,29 +110,28 @@ def test_signed_cooperative_offset_is_valid_data(): "cooperative_mode_disabled", )) def test_unusable_diagnostics_keep_legacy_warning(scenario): - cp, output = make_case() + cp, output, diagnostics = make_case() output_healthy = True now_nanos = FRESH_TIME_NANOS if scenario == "default_message": - output = car.CarOutput.new_message() - output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE + diagnostics = messaging.new_message("starpilotCarControl") elif scenario == "invalid_flag": - output.actuatorsOutput.steeringLimitInfo.valid = False + diagnostics.starpilotCarControl.steeringLimitInfo.valid = False elif scenario == "zero_timestamp": - output.actuatorsOutput.steeringLimitInfo.monoTime = 0 + diagnostics.starpilotCarControl.steeringLimitInfo.monoTime = 0 elif scenario == "future_timestamp": - output.actuatorsOutput.steeringLimitInfo.monoTime = now_nanos + 1 + diagnostics.starpilotCarControl.steeringLimitInfo.monoTime = now_nanos + 1 elif scenario == "stale_timestamp": - output.actuatorsOutput.steeringLimitInfo.monoTime = now_nanos - MAX_DIAGNOSTIC_AGE_NANOS - 1 + diagnostics.starpilotCarControl.steeringLimitInfo.monoTime = now_nanos - MAX_DIAGNOSTIC_AGE_NANOS - 1 elif scenario == "unhealthy_output": output_healthy = False elif scenario == "unsupported_model": - cp, output = make_case(CAR.TESLA_MODEL_Y) + cp, output, diagnostics = make_case(CAR.TESLA_MODEL_Y) elif scenario == "cooperative_mode_disabled": - cp, output = make_case(cooperative_enabled=False) + cp, output, diagnostics = make_case(cooperative_enabled=False) - assert detect(cp, output, output_healthy=output_healthy, now_nanos=now_nanos) + assert detect(cp, output, diagnostics, output_healthy=output_healthy, now_nanos=now_nanos) @pytest.mark.parametrize(("requested_angle", "expected"), ( @@ -134,10 +139,10 @@ def test_unusable_diagnostics_keep_legacy_warning(scenario): (OUTPUT_ANGLE - NEXT_FLOAT32_AFTER_2_5, True), )) def test_fallback_preserves_legacy_strict_threshold(requested_angle, expected): - cp, output = make_case() - output.actuatorsOutput.steeringLimitInfo.valid = False + cp, output, diagnostics = make_case() + diagnostics.starpilotCarControl.steeringLimitInfo.valid = False - assert detect(cp, output, requested_angle=requested_angle) is expected + assert detect(cp, output, diagnostics, requested_angle=requested_angle) is expected @pytest.mark.parametrize("field", NUMERIC_FIELDS) @@ -147,15 +152,15 @@ def test_fallback_preserves_legacy_strict_threshold(requested_angle, expected): (-math.inf, OUTPUT_ANGLE, False), )) def test_nonfinite_diagnostic_values_use_legacy_fallback(field, bad_value, requested_angle, legacy_result): - cp, output = make_case() - setattr(output.actuatorsOutput.steeringLimitInfo, field, bad_value) + cp, output, diagnostics = make_case() + setattr(diagnostics.starpilotCarControl.steeringLimitInfo, field, bad_value) - assert detect(cp, output, requested_angle=requested_angle) is legacy_result + assert detect(cp, output, diagnostics, requested_angle=requested_angle) is legacy_result @pytest.mark.parametrize("field", ERROR_FIELDS) def test_negative_error_values_use_legacy_fallback(field): - cp, output = make_case() - setattr(output.actuatorsOutput.steeringLimitInfo, field, -0.1) + cp, output, diagnostics = make_case() + setattr(diagnostics.starpilotCarControl.steeringLimitInfo, field, -0.1) - assert detect(cp, output) + assert detect(cp, output, diagnostics) diff --git a/selfdrive/controls/tests/test_steering_saturation_publish.py b/selfdrive/controls/tests/test_steering_saturation_publish.py index 0c46e77947..0d7623483d 100644 --- a/selfdrive/controls/tests/test_steering_saturation_publish.py +++ b/selfdrive/controls/tests/test_steering_saturation_publish.py @@ -1,5 +1,6 @@ from types import SimpleNamespace +import cereal.messaging as messaging import pytest from cereal import car, custom, log @@ -27,7 +28,7 @@ class CapturePubMaster: class PublishSubMaster: - def __init__(self, car_output, selfdrive_time_nanos, output_healthy=True): + def __init__(self, car_output, starpilot_car_control, selfdrive_time_nanos, output_healthy=True): car_state = car.CarState.new_message() car_state.canValid = True @@ -43,6 +44,7 @@ class PublishSubMaster: "starpilotCarState": custom.StarPilotCarState.new_message().as_reader(), "selfdriveState": selfdrive_state.as_reader(), "carOutput": car_output.as_reader(), + "starpilotCarControl": starpilot_car_control.as_reader(), "driverAssistance": log.DriverAssistance.new_message().as_reader(), "driverMonitoringState": log.DriverMonitoringState.new_message().as_reader(), } @@ -69,19 +71,24 @@ def make_car_params(): def make_car_output(real_limit_error=0.0): output = car.CarOutput.new_message() output.actuatorsOutput.steeringAngleDeg = OUTPUT_ANGLE - info = output.actuatorsOutput.steeringLimitInfo + return output + + +def make_starpilot_car_control(real_limit_error=0.0): + message = messaging.new_message("starpilotCarControl", valid=True) + info = message.starpilotCarControl.steeringLimitInfo info.valid = True info.monoTime = SAMPLE_TIME_NANOS info.cooperativeOffsetDeg = 5.5 info.modelLimitErrorDeg = real_limit_error info.combinedLimitErrorDeg = real_limit_error - return output + return message -def make_controls(car_output, selfdrive_time_nanos, output_healthy=True): +def make_controls(car_output, starpilot_car_control, selfdrive_time_nanos, output_healthy=True): controls = Controls.__new__(Controls) controls.CP = make_car_params() - controls.sm = PublishSubMaster(car_output, selfdrive_time_nanos, output_healthy) + controls.sm = PublishSubMaster(car_output, starpilot_car_control, selfdrive_time_nanos, output_healthy) controls.pm = CapturePubMaster() controls.curvature = 0.0 controls.calibrated_pose = None @@ -100,7 +107,9 @@ def run_publish(monkeypatch, replay, selfdrive_time_nanos, host_time_nanos=HOST_ output_healthy=True, real_limit_error=0.0): monkeypatch.setattr(controlsd, "REPLAY", replay, raising=False) monkeypatch.setattr(controlsd.time, "monotonic_ns", lambda: host_time_nanos) - controls = make_controls(make_car_output(real_limit_error), selfdrive_time_nanos, output_healthy) + controls = make_controls( + make_car_output(real_limit_error), make_starpilot_car_control(real_limit_error), selfdrive_time_nanos, output_healthy, + ) cc = car.CarControl.new_message() cc.enabled = True cc.latActive = True diff --git a/selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py b/selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py index 24f9fb268d..60785f3240 100644 --- a/selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py +++ b/selfdrive/selfdrived/tests/test_tesla_coop_saturation_warning.py @@ -53,6 +53,20 @@ def make_car_control(requested_angle, lat_active=True): return control +def make_starpilot_car_control(controller): + message = messaging.new_message("starpilotCarControl", valid=True) + values = controller.get_steering_limit_info() + info = message.starpilotCarControl.steeringLimitInfo + info.valid = values["valid"] + info.modelLimitErrorDeg = values["modelLimitErrorDeg"] + info.resumeLimitErrorDeg = values["resumeLimitErrorDeg"] + info.cooperativeLimitErrorDeg = values["cooperativeLimitErrorDeg"] + info.cooperativeOffsetDeg = values["cooperativeOffsetDeg"] + info.monoTime = values["monoTime"] + info.combinedLimitErrorDeg = values["combinedLimitErrorDeg"] + return message + + def run_controller_frame(controller, requested_angle, torque, speed, measured_angle, frame, lat_active=True, steering_disengage=False): now_nanos = START_NANOS + frame * 10_000_000 @@ -65,7 +79,8 @@ def run_controller_frame(controller, requested_angle, torque, speed, measured_an event = messaging.new_message("carOutput", valid=True) event.carOutput.actuatorsOutput = actuators restored = messaging.log_from_bytes(event.to_bytes()) - return restored.carOutput, now_nanos + diagnostics = messaging.log_from_bytes(make_starpilot_car_control(controller).to_bytes()) + return restored.carOutput, diagnostics, now_nanos def make_lateral_car_state(speed, measured_angle): @@ -163,13 +178,13 @@ def test_recorded_light_torque_reproduction_no_longer_reaches_warning(): output = None now_nanos = 0 for frame in range(260): - output, now_nanos = run_controller_frame( + output, diagnostics, now_nanos = run_controller_frame( tesla_controller, REPRO_REQUESTED_ANGLE, REPRO_TORQUE, REPRO_SPEED, REPRO_MEASURED_ANGLE, frame, ) if frame >= 210: legacy_limited = abs(REPRO_REQUESTED_ANGLE - output.actuatorsOutput.steeringAngleDeg) > 2.5 corrected_limited = is_angle_steering_limited( - CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, now_nanos, + CP.as_reader(), REPRO_REQUESTED_ANGLE, output, diagnostics, True, now_nanos, ) baseline_log = advance_angle_counter( baseline_counter, CP, lateral_state, legacy_limited, REPRO_DESIRED_CURVATURE, @@ -178,7 +193,7 @@ def test_recorded_light_torque_reproduction_no_longer_reaches_warning(): corrected_counter, CP, lateral_state, corrected_limited, REPRO_DESIRED_CURVATURE, ) - info = output.actuatorsOutput.steeringLimitInfo + info = diagnostics.starpilotCarControl.steeringLimitInfo errors = (info.modelLimitErrorDeg, info.resumeLimitErrorDeg, info.cooperativeLimitErrorDeg, info.combinedLimitErrorDeg) assert info.valid @@ -213,13 +228,13 @@ def test_persistent_real_model_limiting_with_light_torque_still_warns(): angle_log = None output = None for frame in range(60): - output, now_nanos = run_controller_frame( + output, diagnostics, now_nanos = run_controller_frame( tesla_controller, requested_angle, REPRO_TORQUE, speed, 0.0, frame, ) - limited = is_angle_steering_limited(CP.as_reader(), requested_angle, output, True, now_nanos) + limited = is_angle_steering_limited(CP.as_reader(), requested_angle, output, diagnostics, True, now_nanos) angle_log = advance_angle_counter(angle_counter, CP, lateral_state, limited, 0.01) - info = output.actuatorsOutput.steeringLimitInfo + info = diagnostics.starpilotCarControl.steeringLimitInfo assert info.valid assert info.modelLimitErrorDeg > 2.5 assert info.combinedLimitErrorDeg > 2.5 @@ -238,18 +253,18 @@ def test_default_old_output_uses_legacy_warning_path(): output_event = messaging.new_message("carOutput", valid=True) output_event.carOutput.actuatorsOutput.steeringAngleDeg = REPRO_MEASURED_ANGLE output = messaging.log_from_bytes(output_event.to_bytes()).carOutput + diagnostics = messaging.log_from_bytes(messaging.new_message("starpilotCarControl").to_bytes()) lateral_state = make_lateral_car_state(REPRO_SPEED, REPRO_MEASURED_ANGLE) angle_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL) for frame in range(50): limited = is_angle_steering_limited( - CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, START_NANOS + frame * 10_000_000, + CP.as_reader(), REPRO_REQUESTED_ANGLE, output, diagnostics, True, START_NANOS + frame * 10_000_000, ) angle_log = advance_angle_counter( angle_counter, CP, lateral_state, limited, REPRO_DESIRED_CURVATURE, ) - assert not output.actuatorsOutput.steeringLimitInfo.valid assert angle_log.saturated selfdrived = configure_selfdrived(CP) events, _ = run_selfdrived_warning_path( @@ -265,15 +280,15 @@ def test_stale_real_offset_sample_uses_legacy_warning_path(): angle_counter = LatControlAngle(CP.as_reader(), None, DT_CTRL) for frame in range(260): - output, now_nanos = run_controller_frame( + output, diagnostics, now_nanos = run_controller_frame( tesla_controller, REPRO_REQUESTED_ANGLE, REPRO_TORQUE, REPRO_SPEED, REPRO_MEASURED_ANGLE, frame, ) - assert not is_angle_steering_limited(CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, now_nanos) - stale_now_nanos = output.actuatorsOutput.steeringLimitInfo.monoTime + 100_000_001 + assert not is_angle_steering_limited(CP.as_reader(), REPRO_REQUESTED_ANGLE, output, diagnostics, True, now_nanos) + stale_now_nanos = diagnostics.starpilotCarControl.steeringLimitInfo.monoTime + 100_000_001 for _ in range(50): limited = is_angle_steering_limited( - CP.as_reader(), REPRO_REQUESTED_ANGLE, output, True, stale_now_nanos, + CP.as_reader(), REPRO_REQUESTED_ANGLE, output, diagnostics, True, stale_now_nanos, ) angle_log = advance_angle_counter( angle_counter, CP, lateral_state, limited, REPRO_DESIRED_CURVATURE, @@ -291,24 +306,24 @@ def test_inactive_interval_clears_diagnostics_before_reengagement(): CP = make_params() tesla_controller = CarController(DBC[CAR.TESLA_MODEL_3], CP) - active, _ = run_controller_frame(tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 0) - inactive, _ = run_controller_frame( + active, active_diagnostics, _ = run_controller_frame(tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 0) + inactive, inactive_diagnostics, _ = run_controller_frame( tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 1, lat_active=False, ) - resumed, resumed_now = run_controller_frame( + resumed, resumed_diagnostics, resumed_now = run_controller_frame( tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 2, ) - overridden, _ = run_controller_frame( + overridden, overridden_diagnostics, _ = run_controller_frame( tesla_controller, 0.0, REPRO_TORQUE, REPRO_SPEED, 0.0, 3, steering_disengage=True, ) - assert active.actuatorsOutput.steeringLimitInfo.valid - assert not inactive.actuatorsOutput.steeringLimitInfo.valid - assert inactive.actuatorsOutput.steeringLimitInfo.monoTime == 0 - assert resumed.actuatorsOutput.steeringLimitInfo.valid - assert resumed.actuatorsOutput.steeringLimitInfo.monoTime == resumed_now - assert not overridden.actuatorsOutput.steeringLimitInfo.valid - assert overridden.actuatorsOutput.steeringLimitInfo.monoTime == 0 + assert active_diagnostics.starpilotCarControl.steeringLimitInfo.valid + assert not inactive_diagnostics.starpilotCarControl.steeringLimitInfo.valid + assert inactive_diagnostics.starpilotCarControl.steeringLimitInfo.monoTime == 0 + assert resumed_diagnostics.starpilotCarControl.steeringLimitInfo.valid + assert resumed_diagnostics.starpilotCarControl.steeringLimitInfo.monoTime == resumed_now + assert not overridden_diagnostics.starpilotCarControl.steeringLimitInfo.valid + assert overridden_diagnostics.starpilotCarControl.steeringLimitInfo.monoTime == 0 @pytest.mark.parametrize(("fault_field", "expected_event"), (