From 9292b81de08eaf93cd3167d7e56697e9f664adc6 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 9 Oct 2025 09:53:41 -0500 Subject: [PATCH] New Lateral Changes --- cereal/car.capnp | 1 + cereal/gen/cpp/car.capnp.c++ | 101 ++++++++++-------- cereal/gen/cpp/car.capnp.h | 21 +++- cereal/libcereal_shared.so | Bin 724176 -> 724176 bytes .../lib/neural_network_feedforward.py | 24 ++--- frogpilot/system/frogpilot_stats.py | 25 ++++- selfdrive/car/interfaces.py | 1 + selfdrive/controls/controlsd.py | 10 +- selfdrive/controls/lib/latcontrol.py | 20 ++-- selfdrive/controls/lib/latcontrol_angle.py | 6 +- selfdrive/controls/lib/latcontrol_pid.py | 6 +- selfdrive/controls/lib/latcontrol_torque.py | 65 +++++++---- selfdrive/controls/lib/pid.py | 10 +- selfdrive/controls/radard.py | 19 +++- 14 files changed, 191 insertions(+), 118 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index f0522253e..056cbf8b0 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -532,6 +532,7 @@ struct CarParams { useSteeringAngle @0 :Bool; kp @1 :Float32; ki @2 :Float32; + kd @8 :Float32; friction @3 :Float32; kf @4 :Float32; steeringAngleDeadzoneDeg @5 :Float32; diff --git a/cereal/gen/cpp/car.capnp.c++ b/cereal/gen/cpp/car.capnp.c++ index 5ad5d4652..0e1555736 100644 --- a/cereal/gen/cpp/car.capnp.c++ +++ b/cereal/gen/cpp/car.capnp.c++ @@ -5262,17 +5262,17 @@ const ::capnp::_::RawSchema s_9622723fcbd14c2e = { 0, 5, i_9622723fcbd14c2e, nullptr, nullptr, { &s_9622723fcbd14c2e, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE -static const ::capnp::_::AlignedData<147> b_80366e0e804ecc1d = { +static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = { { 0, 0, 0, 0, 5, 0, 6, 0, 29, 204, 78, 128, 14, 110, 54, 128, - 20, 0, 0, 0, 1, 0, 4, 0, + 20, 0, 0, 0, 1, 0, 5, 0, 218, 169, 170, 144, 36, 55, 105, 140, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 66, 1, 0, 0, 37, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 33, 0, 0, 0, 199, 1, 0, 0, + 33, 0, 0, 0, 255, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 99, 97, 114, 46, 99, 97, 112, 110, @@ -5281,63 +5281,70 @@ static const ::capnp::_::AlignedData<147> b_80366e0e804ecc1d = { 114, 97, 108, 84, 111, 114, 113, 117, 101, 84, 117, 110, 105, 110, 103, 0, 0, 0, 0, 0, 1, 0, 1, 0, - 32, 0, 0, 0, 3, 0, 4, 0, + 36, 0, 0, 0, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 209, 0, 0, 0, 138, 0, 0, 0, + 237, 0, 0, 0, 138, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 212, 0, 0, 0, 3, 0, 1, 0, - 224, 0, 0, 0, 2, 0, 1, 0, + 240, 0, 0, 0, 3, 0, 1, 0, + 252, 0, 0, 0, 2, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 221, 0, 0, 0, 26, 0, 0, 0, + 249, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 216, 0, 0, 0, 3, 0, 1, 0, - 228, 0, 0, 0, 2, 0, 1, 0, + 244, 0, 0, 0, 3, 0, 1, 0, + 0, 1, 0, 0, 2, 0, 1, 0, 2, 0, 0, 0, 2, 0, 0, 0, 0, 0, 1, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 225, 0, 0, 0, 26, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 220, 0, 0, 0, 3, 0, 1, 0, - 232, 0, 0, 0, 2, 0, 1, 0, - 3, 0, 0, 0, 3, 0, 0, 0, - 0, 0, 1, 0, 3, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 229, 0, 0, 0, 74, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 228, 0, 0, 0, 3, 0, 1, 0, - 240, 0, 0, 0, 2, 0, 1, 0, - 4, 0, 0, 0, 4, 0, 0, 0, - 0, 0, 1, 0, 4, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 237, 0, 0, 0, 26, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 232, 0, 0, 0, 3, 0, 1, 0, - 244, 0, 0, 0, 2, 0, 1, 0, - 5, 0, 0, 0, 5, 0, 0, 0, - 0, 0, 1, 0, 5, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 241, 0, 0, 0, 202, 0, 0, 0, + 253, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 248, 0, 0, 0, 3, 0, 1, 0, 4, 1, 0, 0, 2, 0, 1, 0, - 6, 0, 0, 0, 6, 0, 0, 0, - 0, 0, 1, 0, 6, 0, 0, 0, + 4, 0, 0, 0, 3, 0, 0, 0, + 0, 0, 1, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 1, 0, 0, 122, 0, 0, 0, + 1, 1, 0, 0, 74, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 3, 0, 1, 0, 12, 1, 0, 0, 2, 0, 1, 0, - 7, 0, 0, 0, 7, 0, 0, 0, + 5, 0, 0, 0, 4, 0, 0, 0, + 0, 0, 1, 0, 4, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 9, 1, 0, 0, 26, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 4, 1, 0, 0, 3, 0, 1, 0, + 16, 1, 0, 0, 2, 0, 1, 0, + 6, 0, 0, 0, 5, 0, 0, 0, + 0, 0, 1, 0, 5, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 13, 1, 0, 0, 202, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 20, 1, 0, 0, 3, 0, 1, 0, + 32, 1, 0, 0, 2, 0, 1, 0, + 7, 0, 0, 0, 6, 0, 0, 0, + 0, 0, 1, 0, 6, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 29, 1, 0, 0, 122, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 28, 1, 0, 0, 3, 0, 1, 0, + 40, 1, 0, 0, 2, 0, 1, 0, + 8, 0, 0, 0, 7, 0, 0, 0, 0, 0, 1, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 9, 1, 0, 0, 122, 0, 0, 0, + 37, 1, 0, 0, 122, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 8, 1, 0, 0, 3, 0, 1, 0, - 20, 1, 0, 0, 2, 0, 1, 0, + 36, 1, 0, 0, 3, 0, 1, 0, + 48, 1, 0, 0, 2, 0, 1, 0, + 3, 0, 0, 0, 8, 0, 0, 0, + 0, 0, 1, 0, 8, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 45, 1, 0, 0, 26, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 40, 1, 0, 0, 3, 0, 1, 0, + 52, 1, 0, 0, 2, 0, 1, 0, 117, 115, 101, 83, 116, 101, 101, 114, 105, 110, 103, 65, 110, 103, 108, 101, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5403,6 +5410,14 @@ static const ::capnp::_::AlignedData<147> b_80366e0e804ecc1d = { 0, 0, 0, 0, 0, 0, 0, 0, 108, 97, 116, 65, 99, 99, 101, 108, 79, 102, 102, 115, 101, 116, 0, 0, + 10, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 10, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 107, 100, 0, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -5413,11 +5428,11 @@ static const ::capnp::_::AlignedData<147> b_80366e0e804ecc1d = { }; ::capnp::word const* const bp_80366e0e804ecc1d = b_80366e0e804ecc1d.words; #if !CAPNP_LITE -static const uint16_t m_80366e0e804ecc1d[] = {3, 4, 2, 1, 6, 7, 5, 0}; -static const uint16_t i_80366e0e804ecc1d[] = {0, 1, 2, 3, 4, 5, 6, 7}; +static const uint16_t m_80366e0e804ecc1d[] = {3, 8, 4, 2, 1, 6, 7, 5, 0}; +static const uint16_t i_80366e0e804ecc1d[] = {0, 1, 2, 3, 4, 5, 6, 7, 8}; const ::capnp::_::RawSchema s_80366e0e804ecc1d = { - 0x80366e0e804ecc1d, b_80366e0e804ecc1d.words, 147, nullptr, m_80366e0e804ecc1d, - 0, 8, i_80366e0e804ecc1d, nullptr, nullptr, { &s_80366e0e804ecc1d, nullptr, nullptr, 0, 0, nullptr }, false + 0x80366e0e804ecc1d, b_80366e0e804ecc1d.words, 162, nullptr, m_80366e0e804ecc1d, + 0, 9, i_80366e0e804ecc1d, nullptr, nullptr, { &s_80366e0e804ecc1d, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE static const ::capnp::_::AlignedData<151> b_c342cefc303e9b8e = { diff --git a/cereal/gen/cpp/car.capnp.h b/cereal/gen/cpp/car.capnp.h index 8fa546740..dbd41180a 100644 --- a/cereal/gen/cpp/car.capnp.h +++ b/cereal/gen/cpp/car.capnp.h @@ -626,7 +626,7 @@ struct CarParams::LateralTorqueTuning { class Pipeline; struct _capnpPrivate { - CAPNP_DECLARE_STRUCT_HEADER(80366e0e804ecc1d, 4, 0) + CAPNP_DECLARE_STRUCT_HEADER(80366e0e804ecc1d, 5, 0) #if !CAPNP_LITE static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; } #endif // !CAPNP_LITE @@ -3148,6 +3148,8 @@ public: inline float getLatAccelOffset() const; + inline float getKd() const; + private: ::capnp::_::StructReader _reader; template @@ -3200,6 +3202,9 @@ public: inline float getLatAccelOffset(); inline void setLatAccelOffset(float value); + inline float getKd(); + inline void setKd(float value); + private: ::capnp::_::StructBuilder _builder; template @@ -7847,6 +7852,20 @@ inline void CarParams::LateralTorqueTuning::Builder::setLatAccelOffset(float val ::capnp::bounded<7>() * ::capnp::ELEMENTS, value); } +inline float CarParams::LateralTorqueTuning::Reader::getKd() const { + return _reader.getDataField( + ::capnp::bounded<8>() * ::capnp::ELEMENTS); +} + +inline float CarParams::LateralTorqueTuning::Builder::getKd() { + return _builder.getDataField( + ::capnp::bounded<8>() * ::capnp::ELEMENTS); +} +inline void CarParams::LateralTorqueTuning::Builder::setKd(float value) { + _builder.setDataField( + ::capnp::bounded<8>() * ::capnp::ELEMENTS, value); +} + inline bool CarParams::LongitudinalPIDTuning::Reader::hasKpBP() const { return !_reader.getPointerField( ::capnp::bounded<0>() * ::capnp::POINTERS).isNull(); diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so index bf57e0cd3bbb18ffaf16bc84259efa09e3c06afd..52e9984afa8e5e71d06170ba455192650822e89f 100755 GIT binary patch delta 21798 zcmeHvdt6l2+W+2M=Hjgx6^8+oK{HX2iwJ?297+W&4LX{bbp*3g(*%!ov{Jy-sJ9Z^ z>B7=NvtBec>{MW9EXV%R6exKW3 zd+j}YKhK(}Srt{YDylf!Ui`jAkz4Wd>7P5Bwiuo3?^?3@R@d`eqJsAao-LV4TVVZL^PHT9<$HBF2A$%Qukr&KjtTaM4!+J>JdyQ@YST4_CewW_p;BQa{RmU+a5 zcKVSRchP7=G9%nqvPj!{WHMx&N2O%nqp&&GkSz1~N>*uWk4{#nY3eZ-TKBP3HA&0C z=Ok@DK9_0R@wrUfcPz%esKGc=B|UvDRB91FyWEw9hW#oN`N&di)ZD>7Mh8D}5??^xWp_ zF{#whjco%@GvV{J?Z3FDr>!TCCYJGP6TXqX_wn`aeTTfafv0I}kGr(2n3Znp`6tQo@>HOHRlPJdwrRqRc&JHv=p#)TMV|@ z#FlA!CtL%|NjI&pZzS1U4Xx~(aP5;5(e6r8D>BhlntIZe@A%MAZZNTox0>*3vX;@$ zH;A;4D2O)jc_#cj!;U6SI@9aPW{pWEcN@jl2HtAIk83R_UGCEyVSnEUo3)5jE_d@M zhRt~<_KcQ!$~Eu;M`B`gKQ*+PZo;+KPerF+CN)Qtugl;KhFX>hFWzC`)6BT$JneEP zl2)CGtS!sn8mDihv(LATRcsWB5;;aS(fMeKf%uX{(U;XeB&T-$jz z+WjJ_6`ANt&3Vq{t_m=2;TueBm6ihb80p#)d|h79^1xQw4c%-LTdA!*=gPOdM}ur) zmGGc_-bzcKdl}K7Cf^nPehKx5$Jia?B}Q`_F^N>A%&_-!?@j5~(-`mG^r9uxk(7I6vRWBm>)$9$;95lF_`c)j^Qrf7;Fz4cCv25wI6?vb~j}kbb*OJu0>pPg?OhL zY=epQPSx(a7OkGt3a{Z=iA-dS@bz4vZ3SEKh+$%$iEY*z;UH_e!8V!Ltm!K{tI_#A zatzur(%1AL!m~{HyxE5Qv^MYp6F!>k)S2++xw@UsN~@VzQLDzNi-oGHuJl()d)g@9 z=p6HnDCV_+7n|@>kppuB*O9p<6CL|B4MM7~o7kttMpcb=uP41E6JI7;VAfsvjGWSFiEwF6n|;t1tX?16Wt{8K<8Vg7DhR#zjtm8k66H)Ur&phwxGp?pS7+D{lj@HsL+Vj^`fV zusp@OoqQ$szY|KD36JDJO?Wg1YQocAH3F?{1FzO`@k*fDGj0&sXfVk>O?c!O-{6}? zV<0A2v$zz9306f?(@au3M2ZCytn6JQkSY^hCh{a*P1;Q+y6O3~78Nf)y!1~DwZ__B&ZctjR6;4R|Cbja|64Juql4Q;=}I)yRKXi7%L*5^t)AkQEXo1 z7b@x|slkDlP+ABPFVFQ46=ffwe^u{)=JXF$14P7X)fK3!iZWUZ*c%usnx?^)qft_=k2Ksdn*-!Ze1nty|6-Lt)y8eo)ctqxOzwp5B=sqpF4}@mf z0!s(+ER+I!>)=Dv{KCb|g_c1B+@-~r zuZ@6%8oX!W-Ld@gwV8O&5-)GD^yxPaaIE29ao{})uUueW!aR7FpH~-)`Yo28fmPrF zM4oCLE(%m@?Ei26AC5s(h2t|zS=ROX84iQK{b%s|_ne*3?TuHA!smW*v?<)ditkw{ZvmcEZGpN^0TQ zV+}$LmT5&` zgvuH$b+^CYwet0VzC{;f#a6&-tB43g6s;mT3{h}VZPB9Q17z)~xH*Ha;p#b_QiWve#8tBr?MhWwF>EZbj8iM0n7v63ab^xk_w6 z&pGX{hUvEtz7RI#gTjxdiM|nllja+6^gXq^hR^A_GD&Oz>@IuOfV+P^wxH7!i3QwW zC*pcqLj&yyV3DZpWlc+>s@p4X+!1?a_nrUZDxBaD>HRgTamlU~uXUoMNADZQ-|u2;>N-91t35PX0Cm6c*^Z>!6hLA4}Yi`TP6*P9{40bByh z9Qs_pX+N#H#{Dpp!EzaoX#7s?BL^tT9|bYdvfDC1)bz31`f_U)vQ1+m&QQj)&tPh)|_i9lXWu1`BMX!yG32z*TzxpEgmzcz<*rlWL_N3m}Wv*{c%=$Kd(wf&d zaNj&)$8XqWIbx@EbtjjycK`LCg^%8U@!MvR(;t)Uj7j=}E`1l5usF2#pDgO-e&Epe zj0eb(z*g7a7$B}`N%$7LP|MQ4&2bi4qPT#S`S@h@tv)k$ao>De$WoSQ=;a@&&#xg| zyXrCU!44Zfsd$b1<{jo*mW)U4Flt!Wefy(urBBlM4UxZIi`u)fk}Fw*X%Dgo7<<4a zEICh|c%gK4-O3QISJ~crGwJwgR}uH;L-59p>r@hxhYt0r>qeYhpjv+Uwpo|{@$cFu zu{_%9>cW*NsUt7CAK$3Mg{iK;{O4E4?N6+$6wVmva79Y$$V=_s8+3+n;i#u0P4o{&@L`<+oc?om^&0 z9aM7bp1)7}XMtEAi`mANC6?}_ODA&y>Ec)AFL#UR)X06Wl)GW$!jXievk+a~ALSu= zYY=z#QtPUfUyOT?%SEe5wTS9MTqlx~L>}~5LmlNHUH$Qc+rKI9zWF?thu$FdU@F{j zU5M2)40V)$px!%f$idBb%)GO@n9D(rk$M@aa~()tKjb8vp{qx6xhL@WmDU5+{Y~6Y zAa#C|WuBE2>D+KqaYqS%{nDSmD%!ZMS=7c`T>-MFBMKI&#u?;~T8_SFk%bH2`KtdL z+qkx~lIDIIm1(%BBlpaOSEU}BldCzy%l|TN%8p^LRf)zyIEY-vk&($^ykJOk{bs_2 ze!ai`PhNGU$VtGxiABT>!^My5G*3+mKQ+;o`@{9Vv~t77^%)7tNf{=VCt*Hu>E+jn z>WibhWq(sHauN{=S6-yFJP#6e>0~aj+3Bux;a9moneEa$7f@tZs^{wJ zZmyGj_4|yw#?Bh}2KOHn&Y`$Jlae^AFgVFv6?uB;@o^t)KD3PclcdEPHm;9INU9eb zj<_(wk{von+B^3w`m9LgC1Z?Bs3IWuPEL^3hEpzybe;G0&N+LZ-OYVI>DXE+sms2+ zynUdn{~PE zKKyjb7kXc74D2Ew8-`jZ_@mOH8!sLz?}e!B!M70wRNvrwhcqq^K%{6HZcUBig2vsS zCk*QSxUP}D>%newc3*f`?`wqtT>@kUdMlp33CQC9%z@oZ5Q{mZxe zYzY$T*o>8FD=MhbX-y~ z7H-b(dj~iUl<)oUYwmZ`cYfu!mPjY3`uBI5(eYSQd_lEXo@RCVBgHS`9#CDv(azt- z&&(F2_SJN2pT0#)jN|)~^oPxsZro>c%3lU82MO0tc}O_bUp~&76SV8&YkFV5n8_^v z9~ZMIPVP@k*?Tr^@R#ej&(l=;lABEG|DiU7bNS=fXYPx7>YI1CA8$xsZ#ji80eKve zy60J7adW<&rQZ(7y*gy^%a`gz+}$|MIA70G8hQHbx>1~*uNm5};OXfFOS#{ZjnTVI z%Kvdj520MV;$E{R-F~v$Iqr`%sc{Nk!g6SldcW@bF3z{J&Vc>l{v)i@)-pa`?kTo1 z%CU3KUG7LPypU_ZZp_`h-uEgtc;bZInS0*a)htrRU=TRtE_J2Z9XIGYIp4m$q+j>b zUpkkRiS1H{^Xw4UzW|zU(5Z+WYyHd1p$S2D|KOg+6tew@ug_RCt`K}k7XR39btn7) z0bVt}Pzt=u0{g*~lIk|eG@$vxjSmMzhXYo3XhS!0$;O0QZT~0)lhAajO_r0HAK83Ri{FL7i-@SE?^-GJO-weRF_Mp5{>RvhyyaL z*CF8PZP>ORgg`VJUEn_9I0)Ml-FE*n5?CMw*0aE2FucyXQZfx_^wByh{w!5HguwZ2 zhVAHj2;7fG7q|sX88Oo(Q;bIU8ZS0MH8c?dk(AsyBn2#oNZ=LG00DKCxO~tWrdA7A zz4c+WK|E7$jYq!t{d((TYOZh{vW@}xi1GHfL*OdK&xgQeh+_$LY%B@=!IUma4t0Zi`FmB*}IXVqgWaw2pg zc+0zJ?hS*`Au#-#gl&>(M5Ba*ISPieqUR(Nc2-e#$$crpyyd)T3_!~SW?lf(EDDby z;$`^AY0;0RP(2#2G&~p+kQ1XVl5wBI^;#NE0h3!IY7p}DNAa0|P4E~5EX&Z0k@~a1 za0+ytWOkxiDD_u?8AXY~gOUk6uPC#mPAM4vH9;^yix*962$*qV%+FT$;0c)88KnA( z6!;j;c{#vYVE9J{|CUS(nwO=~KZ8js6{SC8gpc6!JklS20aA~l`9w;c0K?hRrzNuv z%?onuJ7LQ64+uVz%t18!q)reR&X+0|!9=3zDRmOSl#7TaYf$J=eB|`#QYo|rO|S<- zxQlDDx&s9SJ`}Sd8(4ympH{rkWVKuT9D(As)7D<kq&W!e;Cc9k}tv8 zE}Vh3;86a-z*focL-QBOzh(E&-YP}}+G0aE2|A}U1YSp@8@OusuR^8P=1%1FXA>xk zql>CkNdwU;<}D=|djYiEjpiH4B!Ef#NF0>TIp?`iN_>arl$7`!67|AivAGjC+v!0$ z(~^cp-@8i?;C$yK$vnr*WiY43pQS2iK5M1GB^DUuXZPpiXNMp#gV5+k6Tn;$o1p5K z`>FWKVjGyi=}mh#2*#la#qCBwsy{CNwElX8WM-m?l*||~#Uk2@QQ`IZpGbkjXwKs) zA)o*PoX z&A+7az7{#93LubxRHYu@Nh#1ZL{(~}Kr93}Z`lW+`mdr*l1>#K@Pvi{cQYo+njjnnM0=)WtyJUVrGefGL1(SPB zoPyghPEX#B8oOmO8eKQZX7>--C?eX~g8g!v#E^EjfqtHiVmA6=yh43;4`>K9JEYBH z&`3HZ-iBltCmlWEkVr#QAthe4V+w2(Ez+jrjA-8;HnTU1;q7g~iM*~n%L9!9G|s-z zu(iXyEkM5D+3IhbSOL8yjaAXUF!5a@^IEvfn?1S&-_ z1QL10_dF?3iso}Ea2W!eJKQ0elW6oQJ+~t!zqpm<%MjosVRwL*k<9!C<^!=Bs(AkY7%mbyE$HqKxfC?|88*&o_uomu zjg!o?X!OhM17OnniEsz@oRfk(rNkLDdU(qq@jdAVMu8cC=8)X66=15xOouIAtrMjV z#LsJ^Tcn&DYh$cIPJ1u`^~AFF?L078-NW%#_Z)m9~b6l+KuyoWQ#(C3?rv*aUUOjNT-c!A2tI z?iPczY(leH+DO5jD`)RMm(0)1+zDpRCUFqDex6a{BD^GWuI|%;kUWBBj@+m7xP7gn z)xH-c6CSTB`=#~Ys+bau`!CLzHAw--ovJcL3e1xCIh$Z0gi~j; zK?Mpl?@RuSywR@*@0T@Qv_@lE^vDrWF$k5{)~>t@q5cESFH(LNZk*=L7TJ&pbyeb-Fz>V4Bm2sFioV&Uu4Fb2L(H;K?0bZdxS~7Fcbn61uMleNUOdQU7BsSyV zsa3pzw=hmnElP*@Ml^|1{2kmaS*C~@xJu+qRI8Nmj3$X6Ai)bv;{jSGp}9-y)`RiB zBzg=)n4EUnASG(itdaZC4YxzQcJzp3I^2z~b9t1)z|@Jy2V&Otm?H3twBST!oAe>8 zfsCaU&BM}3CImUv)czhY@o1it%wNIuoFY!^y2gr6rBF(2V2Q6F!Fi=mC3A!sd7ltkdN0K9LKEH<;)`$-QZ0rivpEZ`&8f#qq;Fo}u*Z*?VQmwY|cB*HA(!u=Ru&i?}^@ z{2XbOA@7yxENth9&Ja1E(|L_o4bdYq46FRKS3p*063)-rNfgCvjEn8k8$b9K5PgK` zT+sqLpa1Y8*f`M2piILXKm4~ESqtSs!ufA6lJImax5b3>LvE`9E1L-Cm02}bgZQ51 zjYV(b`Mq|hn-`h!F5h~e5k!O7x)&qDi^D1@mbpap$|)O-)M~={&m|b&L3p-s{(uqT z6@zu8*7Z+@w+xXYXN_2zBll1lfqoBj^;dSk`l5R~j#@R!|9Wc%?UMy-Q z%?qm>v@7F?jumR1-4()r2~tA7*Awl1r($58JyErDuBfxf+h?Dkek%UG&mN;r6_@te zQ*lF+@*{fHBJW4^)`@LD+P_jei^Bc(;p!-{eZM_5gjY`4=r9eMWQ3SM=>F;aoRuBKh7^j6f@e+9=O!`dwZ*jpLI<^PQfD)YcHr zxeyNP3&xv_iCj%~P7%)KXg+>H4;$`jDO5kDhT?>8ZL^K}7a92^L zkFQxPW*GkJh4UDWJwHM+eUxa6mM{=U-5ASP6vFA3XHaYmm~= zLqu@akONvm_)B8#FBlz8jPs!GA^JN}3p$;bT`}D|kB$Xpz&MOCgioV%1j|23_%RW8 z9Nu`jE04k#L?013lIAi1(?N3#Zz$SJdY;MnBShDT?V!{7F`MVXM?~{OzDEuVwu6Lo z;()yc{)N3ooWPOf)l?2cOBqk}^`BT1zJPGf<+HhW2_X#KkKeqsj3|!WW4~884@>*#8Qm!l}103 z+$Rv8W#IYxdP>PXL6d5$b0oU00qZzlW-4G5o!dkBvt$9!C`63&UY&U!%Y6{Yu6 zMx9zGc4QwLs)an%6FQ$jSXgmx;BY^ZDW3 zLEp&l63wfM_~vjo;k;~%Z*ngX&TBo&=uq}}+K7U)7A!xCa9SXfuauDECy3z1Q}c|W zQC6}7{e;|0^8X;5*NapVevEKlXjHC`Fc$1QV+6#@m$LPf3R5zX2;6-(W4w3;OeEd$ zEG4>5v`Cs4OBIp3y+k*Oh!*>=$mTfDK#Fr;-N^aJ3k>H?B1O{t3vF_NETBOj7kQx5 zIiWF%wC^XHb2U6q7ZT1toM&_I623#!N^hLR>Otl%5dEr9&%zu3ik2tgsC*+BPWZ6Z zCkW@<8Aq{_@FtN1bJu@!&2#-2(amDHqM#b~zkfPuvNUZ!N32hQv z&tW_{X~_f7ZIR)VQ(e4kcM#6Yx_F-+C49Ybp0~UFkpAm;-X86KiT=x!B2zxr#X2f4 w$$Zo@B(2m7_6qevyO_>cl@e!bA7j7%N2g-Ct^Muog2_Pe`xP5)?QaYCKSMbRK>z>% delta 21705 zcmeHvdt6l27XLX6%)?hRC=LUt;B!>uDTt;un8rYA(Di}61bdO0g4euQDqvaIy(!$2 zZn9GB^+Q9&dn?FpiCrtwGN?2xA7H-n75GHU{H=BNTIL)t|NQ>`ozG|X%>I7Y>+G}7 znRC|KGxe`T*1r;2l4Y}RGAp>*OCR6r$k`}6*Wb0_>Y8qCa|Wfm6X)+QdNFHctTOxV z{^NdM@$XaWsh?c6&HlP_eUMduH_5J2tF`W*rZ#FB2b|D8ejwTv^9NB*U&$D4^MPq< zj^;S%gm&P;6p+t@&yCtr{d0r<`Im#yuBwOt-G|lXYoSVuZJMTfwbCXhwCkEcvY(U; z4lzo$>XI4FDEWAEw97k?Btv~Az1rsH6qkF5WUCCUTXP(mrnYE#hn&zZJv7Z_A1Nhk z!hG%6HTCCc)uYA!?1VP;=M*(lD}~RQ+Pa^oxoYl|THc<%S~Xh4;b^s3%Q)l<6mKPk3$aE}51oue|~ErO!@-}t>s zt$`RxCr@xc-+;HtP*CEj1KdJ9wJ`pP{v$a=K=5goAw}%+(@JJ6*Y3q|JN- zJ4een?Tnkpkr>#PkEGV|n{aK_>8RAFNzESV>#~gSOapG;F3sf_am{hY>H2`Q8Vzi^ zmVCxJJ^eGOs|@k=Rm(9L@a-If0k8jD+9|mS*S4IADySp18UuZTa3#vueR;h!XKx30 z8}Lr+qUZ zyfl~I4xVGc&y$^61D<#Ol>g&GblEkxa!nGaeqFfzFt=K>( zX^!(wm&ae;);AegkCqJVqoiw1@O3#)%L6vaCUvt6Y?8M2ytCkW(yceJg&oLaJ9wJ` zFX0#xePeiy@N@$n(?z$h4ZGkdpwCLaL8gLqiw*b(9J2wh2&9 z6r2&)nlCzCZ5*3{RW!#Xrz@r>MPguMv}9nd@5-rbAMP88UCTo@Lh9xkSgW@7QnVVZ zHC%G0hLMFPgG@zlsoT;HZXMz4F9@_mGH$=s*Kaym3pd~swTR2`JvLZcD>krRE#q=DJSrAlcBbZ#g*JmsTeQ@* zj`a1&c(?)2*P1UoL(1c&R+)hFfS;B4V5P!9l#i7T>Tbfd8NWxl0_hQ_ z#Xx(s&A&TSYwwZDUXO3ojAt3}S!AuofTwGYt4>$?L}{(bz%I~|k)1-ml6`%_^Z|Ai zTQ;!y+S;p5HDCMiYIJHj$+Q_{Dhcowy88jC zd)&Z&!Z8@|I*#EE-x!LgOFNl2;o6UXM7eUZBwc8rXJ`@EoFTOjNw&$r);^@&b1h1p zsTEy=XC*R`KE~IxqHP9Nc|@AXH?X-{GdO6>k?e5;+nBQ=P>m{BO{VQ*eNDedc%}hw zn+3sqH}se#$#?M~n5O7bP1-ws}4z{5p0 zn2YO8=8hZas`=8KJ;m2e)qJsDRij*eNiWI3yG1LQbtOF`)k_R?l8EpFI+C<&40M{v z06MkoS#s|6bW9ft4S@>K$jCq=#yHSOSaIxdz4sJ&y~BO6Ty*(-#*$NL7KE1Cmfg5`?K0WiTlbWzMP zNR1K6W|&~^cVr;72HGw1koJ=HaRZ(6!dkNmFF$yhs@IzR8}<(HSNs%JF)J2D-h`^q zH!1$mLrlH{BC`LYcCD^z@()(4ziReBy9%x`TNeZ*h<_{#h!%Yo1_X(=;{o@pA>!*I zzfck5FonVA{XGIgMcY68f>+wj{%V>SwHUZ?_$>C#^6M6G8tN4xV*FD8mJ9)KT%@U+ zh*|3IukH{ZJ4~S>Db6oAKvfmxX|W**OnLf&)T#UZx{1tC|M2Rhf&h=7i20|#^@ZA^ zfa=;J^Uxu#@X{;S#(?2;c+Z6Q5_q2p?~S7KCi8$n-mPV1$-7;H^5!TrXu#i`B zL=nP`p%UJdHEUT)(S)OoLJdZ^mxPzjnB8^k4*#Y`Q3SABC0c_mPI$AqK*-xi3N6U% zHZfeuZuW|$0Gpsnre&)^!X9F=f~YIRGCRoYmZBTxZ8!9bz`kOTU=KiH&xBjc>{VXvs0^peW5{??zu=Dmgy;sJFbpX4{ z7f86rR}%`m%t|cez9wROTS5cydI&xvEMBgmEqnF0nBRAe{xA0hhe+=wRB6Jl1zxK~ zL%1bnI2Y6Y9Y4uGDrM-K+`mF<{8o8`dM5z4OLeZE1c7MZCXWp zffEfcCN=tA4CgXd!`bP}&ow*BxX)h%>^q$E{s8#FtFI`6K^V>ztJA-4TzT2ExR(1F ztVi$e8`RNzjIN%{r7M;$-#>7Jb=SUP#wQbzPE{zbW#OSfGalVcBp0ieeEae@9a5aH zaDM>XrFSe|iKad#w?1&gxn{M#IpNiP@mF8u{t|;4)v9z9?=Bvc`_AR5shQuz-}m~e zb=)@&-HaRd9fxj~uI}JU)ULmMTJ-4TOW)**?7=YG<`|^s$&um;6o=Mv%i=z+dk=ok zxIYdFY>k8E;pUPQ!neSyOpX*+o-&@8rmh(XzV)ouh$~?tgqqX(+G+>Ylw`Rt4=sc9q>A#>nCt=$+~8B z(up(9V(!leksCKIDj{YTog*!GIk<*YdZqu9mp}QkAxD%(!6M>I%txd^54YC2h$9?tckw!yc2FzxIZ!Tm)jcf-c@A%t-L18rh zF6dnPqUxpY5nYLw`H;1_=Exyx;@to&@^yIj6`g;a~FPQ!H@ zJob1*tE4)VXLR)^_HX;Tq{oH}T&{VQ)Pt#z!gU%}uSa#SUn^?&i@kOKhTCS2t}Ef{ z%wwcoLE2oF!3zUUpcA_DNUpvFocO(MpXI0H+)p5Ne#5HE%Bgg1a8Rv9>9y+egI^S{ z|1eiH#9N&HSYr`|i&c3AF+phscP!prwCL?G2EY0tms(cRy!KMngljCgV@`M_tq;Hx zVyuJ9E4Nqub>j5xqh9ui=Ajm6SFWpIY&eedSe{ENQ!Wnb_tkfKbx9&S0m8ys%Acp} ztz1HxcVDm5Q?0o_T<@d38#XSTAcPYWN+vQau8aISRedq7d)C)6B0CYn;<5!G(L?#t1 zxZr`~jK>=a9WoNGY>b$nbfP-+&_B69jV;qV*Eq0?2B5Z~8yL>Tj4%F}amR!w<6h%);PfYyRhJ(wwe;eoveEZHg0Z42yO z{ION!4TrNkm@>S$+c=lPq*E?tbj$y0$DBP2c5$CiPqr3L>e$DtUYxEDPuAHZEY5+s z1LULgT_m1EB?>NRpi{giq=|@I;qpa=j8{L}ups#I#?@TFFn%R)0RxR<$P>ld5pYfq zHsgsS3U7s57OrBfL>bs}sZxi~0x zH1F$>`r%IQXWb|ZpBIQ`5Rc@-K*f&lW2Zkep_cpo4B}WK5aHj@b=#GnDW^J1O}M%u z#`;Xgp?uMh45t|v3vdrnuu?SNX7O@KV0L1{6R%wAb;>R>M#HriYXN_nc5w3l>E*9a zcpzub&u-BGQV!1jqer~*)#;;?%qf0^aT|Bfi9{vu;}wka9juqK_?+{{$nZ5o6y0G- zN# zkFiWi=7hg=p(vZqj~jk(%cn(*ZzrOW3IbdKKqnpH^`7n|h!gz}ynifq`PG9P^}dKW zpmy!dIej$L124xZTCVDEf|R~K{aVJt^rQT~@*E1m)^uzC_K;ak*7Yk14 zXV|{q;@SVmOL`wqO-DE>pb-qQx2|A5t};i#LjP1zx9GC z?lqaS;4D4lr(UKpmYlVWPs06zt%5S&oC(L^`@rjp+~wT2e6LO;rcTM7xqHpdT#-B; zc9}EasD%c{h=w#cc{%gFt!z+_GoL$_xWzV<=8QK8>X-B!UAj7Eg5|F-h4v4!nSy)E zXD{oI@KqQCjWYy33=*IGWN{_bfdH@OUW5Ygu)tTql#}X*$TUIog9|=B|Q00M4G z)f_|t^Fb0Q5KSPUz9_Ejw}h#+!g;{*klHAoIbey0%9d8Cz}Cm5xeotE=C;!&Li>;0hcMZ9s;mN4EPzq zVsRh76^i9Q16V9B;aiQEb{O8W#21G_Aw~opv5dc)QXg&z{EqNGXsI)-woV}%q(bXdxB0hFpz>fVhI|t13q#}^FJtb7@E3J5ZVb0|8(FI zGQG~icLJ(60MoK$lfY}2K=QqzI z6LyXwo)7buGn~-?nx`=HG%%+`(NT!F5I%B(^8*w*0F4I?4~7ZIDb7}8T;~;~4F@R> zm=+=GA>_xWz-M0N{}>3EmqRli^&bR=6Pj-zvjdt%sQ)4`qbbd|ADMs)it;4tlmf%6 z{DT2Bd!T6x0cNZif6U?<;f0yYdCp}h@BuUzaDY>R;l=-7BGU@ZOK9|NU~1Ng@?$W< zyW#U(a((+nka`T7Ehx1Q7|wk@jm#U+EXA?E1`IFke;=9s(CkGWGn@>Z{ZuXihuSuQbZiL1ceTPkDTm$9)&hS6C4ggMIz(4#T8Ht0##x*$ObHhk2Tdxk6UbJzcPRE z@)=7XRjaN!W7%rHrOaOm-{pV{@L?DaMXP@Pf~Eg0evOr4qrcS|!gPqBX=5e6R_O7Z@iw2@pn+da-xb*?hd>1rdBa;A3-Fsp` zI_JdYdX)GUn$sw;6(k-McC*!$z$we{ZXn==M&G;3Aizn>`;d8-nJd7|5)Yy(r!N~& z;4%vg^|P5cgV`wvn4!?Gm*SlAuXLW<4;9SPjM#IV<@*3T%W% zS3L~^yaIh2GQU7G169uf({e5>n2%krd8`iL#Q8~S0qN_=)t&=AmUN1I1Mqvn)&6C}ep(HPzfB)rg6qr{6gm;&oWE7~k+6&*W* z&8BtY_Kw!zL|)sS84enS&^QKyhP4BnY_z&tL*^rB^n;QN%=vZVCD0A!T%s!iB$A=g zPw`xk@DsHi;e;r;Bz^*CY6sD!6C6dz4(byi10K)K^Pwp~Utyi#nn0_u|AEYV(CBe* z1?Ew4Kd6R6qEJ`$>jMIVpjm^epMyY>C;@>)UUi+10_D(bMS&|Iz)8aG$ee;kpVD(X z!vt6_4t9bu;1$&)`hvuCXbz(8Ga$kF!l#gV9U6TMx^#h?p7ml#XE-Lja{44nbnT}q z*HAYIB?3h@NF;J@FdInoQfTxG-9nJyMbWPzvmKgVgFyFLU}}TJrp|CYIUP8@KS(?U zjXuH$K%!~AxQhNc5BOh{ILQ(zx7bX_*NfOL;Gb7F4<7&$+0aZwFB3q5Q-IGQQwfc3 zeIqb!>qPvpr-;GylSE^x%`r1}vG{0z++ zR1HOemA6>qLpkL)b0F|9L!)0!rvM)&CfowEg_C`UP@uygx}IDC0Z#Yz0MI;^ncskk z6R&|PJlKB#E)qGx=Nb%h$>T>|9=CMFV29fUwOQ+Iq<}PSB!GNiN>ppAC3Zre?Sw73cb3*Ar!d|nEjZy!|g{D z@QYTJSe%8EK!8(tK>(UZL8Bk1g}`iiS8N62evoV5i_SiMS9A`98O4i$$HjogTxe#Z zvv1JW2$38Jvyzi>S5cy0ER9W2H<;1yisfJUE%2Q zn%pOWG_Qc>HK;n~1u^A-%>ME;ucC*I1{EDZGKZaOi;F%~o3Z}#i%5)7&hTE%Y(9DKg zJ-mObeMn4?g8MH{g&ju$`)E~}jsj2OeNH792;rRAY@h-JG@Fn=3y(A60Puc|Iif8J zrp4+k5g84k^7_`5cR;A0K=TXA?}Qttwpk(zBtkjM)ng1ujD@CqAV@5M`=eZ;MZ+o2 zsjP!2U>U0_cc4HZ+@J)C=4kxB$QQq(B~D?LfQ-2Y8a?)85bQ0YV?YqrnWn|S7fWBU z5W1o6Y_mT7tM3H)gV3x;!^I#E&siX#Mv9&A7M4bG^HV@>Ei{cNx5giC%${8t3-g9^ zQnz?PU>G#I;~zl)(o(vMG-T#L)4eOGHUnc7<745hhm2(`c*+;A!dn<8pBATr_5^L^0Vfw~8Psr|d?83$oKgb+r2Jh20W{Z*WIG;{arw?&!! zDmPXye$%!nHkP0J;c-ge7Tk?#)s71Pqe20IPLkJoQQr5t+WXW&Aj4-=guT7fR$|D*_(b66Rg zA-!=PppEwJe!}^mDU#qp8Mo&M=jYuv2`iO^^Kz?tizL1$oK~`l7xvg3E?zmtyWIC4 z8APMlyaz^vSA5k_EOUwGB~exysn-bS|9@b7JK>qa@dJzqF9U2OwQhfr-n=3iXQylv?Z!GP8-=p-d@`jNJo9S*A@-wsLOk8De5Va{3Gdk-XpLR7X}%-EXs~>Rge10CX$GI(!Qktp{w&3wYg19p;Q-`+Ax* z!ONYhXc#XL&I^*m8I4~;EkuXDEa&x^!ZK7nw~RV0y}2MFgI=q7T%gm9`E->a~p4McE}u!IclCY^4Q;t`g4W(BmZEZ@LULNvO@Re_UX$qH%nRXujzSC$-lJ=d=ii^%>*ztK~YfbDD6j zGV}2ZdPusbWln>XdWv@x5xh>M3Kv(x_6Xs*VjIMJeHB1C#q~bX>7o^B&N&tPN$r4# zr8`AL9D*wZ-!@m6Bt3~}UQ5AO;)R6sniBZ3*0;9_hm!@&UH@g4Ug!XU3q;3=%}Dd( z2an0vM`SRtY9HtV&UV!MOXDku=5=e~bn<;nIIkQDrr?hd&QGURgm->ay5S0aCIz2B zI8~|dRl++<#}5#}s~j?E@RktH)%Yedw1IG5DW#D7Zo;8d{Il)P-+Fl{u6~cnxHv0N zLFUF2&iA^(gg-?%FNtcTaeAHblIH5P!?u;G8Z4TRz;JsgGsf4f6*Hv22ZiG(9D9D6 zWcmovoWAHF2Se%ngjC~)UY1WFe4fYyb1qJmQBcYfqI1Psr1{@WnBGmaA{sClegb5= z-%RP0evhbxQ!wKR=ifoH{8NNa7O}^`8$T%8N$oSDVdVhOshss;I_OF1jUNhm7e^7! z|76jd%soPQlGujc_=m$B%=<*gh*tE*c`Gw%2h5V*_!rmgZ3y9<|L8*UlL(K4Ts3&( zKOSLkZxfv;ihxe#d??g4^z8!CoEhQ6+GDnif-^OpNPaBgu)GY+74Ty{2fu=7ex&6n zJ|>+0#z9{TffkPt&X08LrgN@z&p&A7uo4KT|63TO7b4Nn14M8Fkpo&ncqg&;7Z@GR zk@KMLCi))H0CcLAp6Z$Imq*89T>v`K7j5JFc*1$@7t23I_@g5B1bE{`tUL;z5&f{p zMw%-EOb5-8-k>gzG_MO|a9Rgbpp=p0Sw!#xseCy!%1TzCUvzs&{u9D^xkwG+ zM+xV}L{<6-D@vDVWI((`DNDaZ!IVrS0`5LrV7z$Ii<|Cvo+mm}v?9$5qKe7g9-?zZ zM62yrNbESyf)uC0x|8z{7E0$iA{lA^!8EymgrKBnh&-TEIk#~qX-_7a(>6R$7ZJ|C zV`p>k5DrUY&>QEodXl+|L9C+g&welpqvp@#JxgWMVi*Qb(aTF^F&k@;R?)p!l zd9EKNI#-k;&CB9=WD*OdH?P=+H2;K`gL$54c!UJHfENIAOnZptJQ0uBAB1y~jK7Zh z7fElN8smMKKsYBSM<{ZeM>wTP#S7;lMZqhOSow1jiWHmA!+3K3k_VvsV(F8!Uc76! z5zcG4c%L36ysvOvusQu86FBICEy~qt6D!KlKG4NFE1g8%1-KaGyr5pRRjU^}L float: return SequenceMatcher(None, s1, s2).ratio() -class LatControlInputs(NamedTuple): - lateral_acceleration: float - roll_compensation: float - vego: float - aego: float class NeuralNetworkFeedforward: def __init__(self, CP, LatControlTorque): @@ -212,7 +204,7 @@ class NeuralNetworkFeedforward: self.nn_future_times = [time + self.lateral_delay for time in self.future_times] self.past_future_len = len(self.past_times) + len(self.nn_future_times) - def compute_nnff(self, CS, VM, actual_lateral_accel, desired_lateral_accel, gravity_adjusted_lateral_accel, lateral_accel_deadzone, llk, measurement, model_data, params, pid_log, roll_compensation, setpoint, frogpilot_toggles): + def compute_nnff(self, CS, VM, actual_lateral_accel, future_desired_lateral_accel, error, gravity_adjusted_future_lateral_accel, llk, measurement, model_data, params, pid_log, setpoint, frogpilot_toggles): if self.use_steering_angle: actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0) actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2 @@ -227,7 +219,7 @@ class NeuralNetworkFeedforward: friction_upper_idx = next((idxs for idxs, value in enumerate(ModelConstants.T_IDXS) if value > lookahead), 16) predicted_lateral_jerk = get_predicted_lateral_jerk(model_data.acceleration.y, self.t_diffs) - desired_lateral_jerk = (interp(self.lateral_delay, ModelConstants.T_IDXS, model_data.acceleration.y) - desired_lateral_accel) / self.lateral_delay + desired_lateral_jerk = (interp(self.lateral_delay, ModelConstants.T_IDXS, model_data.acceleration.y) - future_desired_lateral_accel) / self.lateral_delay lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk) @@ -252,7 +244,7 @@ class NeuralNetworkFeedforward: roll = roll_pitch_adjust(roll, pitch) self.roll_deque.append(roll) - self.lateral_accel_desired_deque.append(desired_lateral_accel) + self.lateral_accel_desired_deque.append(future_desired_lateral_accel) # prepare past and future values # adjust future times to account for longitudinal acceleration @@ -275,16 +267,15 @@ class NeuralNetworkFeedforward: pid_log.error = torque_from_setpoint - torque_from_measurement - error_blend = interp(abs(desired_lateral_accel), [1.0, 2.0], [0.0, 1.0]) + error_blend = interp(abs(future_desired_lateral_accel), [1.0, 2.0], [0.0, 1.0]) if error_blend > 0.0: # blend in stronger error response when in high lat accel torque_from_error = self.lat_torque_nn_model.evaluate([CS.vEgo, setpoint - measurement, lateral_jerk_setpoint - lateral_jerk_measurement, 0.0]) if sign(pid_log.error) == sign(torque_from_error) and abs(pid_log.error) < abs(torque_from_error): pid_log.error = pid_log.error * (1.0 - error_blend) + torque_from_error * error_blend # compute feedforward (same as nn setpoint output) - error = setpoint - measurement friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk - nn_input = [CS.vEgo, desired_lateral_accel, friction_input, roll] + past_lateral_accels_desired + future_lateral_accels + nnff_common + nn_input = [CS.vEgo, future_desired_lateral_accel, friction_input, roll] + past_lateral_accels_desired + future_lateral_accels + nnff_common ff = self.lat_torque_nn_model.evaluate(nn_input) # apply friction override for cars with low NN friction response @@ -296,8 +287,7 @@ class NeuralNetworkFeedforward: pid_log.error = float(torque_from_setpoint - torque_from_measurement) - error = desired_lateral_accel - actual_lateral_accel friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk - ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.lat_control_torque.torque_params) + ff = self.torque_from_lateral_accel(gravity_adjusted_future_lateral_accel, self.lat_control_torque.torque_params) return pid_log, ff diff --git a/frogpilot/system/frogpilot_stats.py b/frogpilot/system/frogpilot_stats.py index a3f4dc963..977dfb961 100644 --- a/frogpilot/system/frogpilot_stats.py +++ b/frogpilot/system/frogpilot_stats.py @@ -97,6 +97,19 @@ def get_city_center(latitude, longitude): print(f"Falling back to (0, 0) for {latitude}, {longitude}") return float(0.0), float(0.0), "N/A", "N/A", "N/A" +def update_branch_commits(now): + points = [] + for branch in ["FrogPilot", "FrogPilot-Staging", "FrogPilot-Testing"]: + try: + response = requests.get(f"https://api.github.com/repos/FrogAi/FrogPilot/commits/{branch}") + response.raise_for_status() + sha = response.json()["sha"] + points.append(Point("branch_commits").field("commit", sha).tag("branch", branch).time(now)) + except Exception as e: + print(f"Failed to fetch commit for {branch}: {e}") + + return points + def is_up_to_date(build_metadata): remote_commit = run_cmd(["git", "ls-remote", "origin", build_metadata.channel], f"Fetched remote commit", "Failed to fetch remote commit", report=False) @@ -144,7 +157,10 @@ def send_stats(): selected_theme = random.choice([item for item, count in most_common if count == max_count]).replace("-user_created", "").replace("_", " ") - point = (Point("user_stats") + now = datetime.now(timezone.utc) + + user_point = ( + Point("user_stats") .field("blocked_user", frogpilot_toggles.block_user) .field("car_make", "GM" if frogpilot_toggles.car_make == "gm" else frogpilot_toggles.car_make.title()) .field("car_model", frogpilot_toggles.car_model) @@ -180,10 +196,13 @@ def send_stats(): .tag("branch", build_metadata.channel) .tag("dongle_id", params.get("FrogPilotDongleId", encoding="utf-8")) - .time(datetime.now(timezone.utc)) + .time(now) ) - InfluxDBClient(org=org_ID, token=token, url=url).write_api(write_options=SYNCHRONOUS).write(bucket=bucket, org=org_ID, record=point) + all_points = [user_point] + update_branch_commits(now) + + client = InfluxDBClient(org=org_ID, token=token, url=url) + client.write_api(write_options=SYNCHRONOUS).write(bucket=bucket, org=org_ID, record=all_points) print("Successfully sent FrogPilot stats!") except Exception as exception: print(f"Failed to send FrogPilot stats: {exception}") diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 7f925f8e6..ccb7b810c 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -305,6 +305,7 @@ class CarInterfaceBase(ABC): tune.torque.kf = 1.0 tune.torque.kp = 1.0 tune.torque.ki = 0.3 + tune.torque.kd = 0.0 tune.torque.friction = params['FRICTION'] tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] tune.torque.latAccelOffset = 0.0 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c19733b87..0911de46c 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -29,6 +29,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, S from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel +from openpilot.frogpilot.tinygrad_modeld.tinygrad_modeld import LAT_SMOOTH_SECONDS from openpilot.system.hardware import HARDWARE @@ -135,11 +136,11 @@ class Controls: self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.LaC = LatControlAngle(self.CP, self.CI) + self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL) elif self.FPCP.lateralTuning.which() == 'pid': - self.LaC = LatControlPID(self.CP, self.CI) + self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL) elif self.FPCP.lateralTuning.which() == 'torque': - self.LaC = LatControlTorque(self.CP, self.FPCP, self.CI) + self.LaC = LatControlTorque(self.CP, self.FPCP, self.CI, DT_CTRL) self.initialized = False self.state = State.disabled @@ -671,11 +672,12 @@ class Controls: # Reset desired curvature to current to avoid violating the limits on engage new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) + lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_safety, self.desired_curvature, - curvature_limited, + curvature_limited, lat_delay, self.sm['liveLocationKalman'], self.sm['modelV2'], self.frogpilot_toggles) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 93f59f276..b4ef85fe7 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,33 +1,31 @@ import numpy as np from abc import abstractmethod, ABC -from openpilot.common.realtime import DT_CTRL - MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s class LatControl(ABC): - def __init__(self, CP, CI): - self.sat_count_rate = 1.0 * DT_CTRL + def __init__(self, CP, CI, dt): + self.dt = dt self.sat_limit = CP.steerLimitTimer - self.sat_count = 0. + self.sat_time = 0. self.sat_check_min_speed = 10. # we define the steer torque scale as [-1.0...1.0] self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, llk, model_data, frogpilot_toggles): + def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, curvature_limited: bool, lat_delay: float, llk, model_data, frogpilot_toggles): pass def reset(self): - self.sat_count = 0. + self.sat_time = 0. def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited): # Saturated only if control output is not being limited by car torque/angle rate limits if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed: - self.sat_count += self.sat_count_rate + self.sat_time += self.dt else: - self.sat_count -= self.sat_count_rate - self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit) - return self.sat_count > (self.sat_limit - 1e-3) + self.sat_time -= self.dt + self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit) + return self.sat_time > (self.sat_limit - 1e-3) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 01efe0868..d2cd07a44 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -8,12 +8,12 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + super().__init__(CP, CI, dt) self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.carName == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, llk, model_data, frogpilot_toggles): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index ed3a62f03..73ed3b17b 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -6,14 +6,14 @@ from openpilot.selfdrive.controls.lib.pid import PIDController class LatControlPID(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CI, dt): + super().__init__(CP, CI, dt) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, llk, model_data, frogpilot_toggles): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index dbc8d638f..35bf53480 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,8 +1,10 @@ import math import numpy as np +from collections import deque from cereal import log -from openpilot.selfdrive.controls.lib.drive_helpers import get_friction +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED, get_friction from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.pid import PIDController @@ -21,20 +23,26 @@ from openpilot.frogpilot.controls.lib.neural_network_feedforward import LOW_SPEE # friction in the steering wheel that needs to be overcome to # move it at all, this is compensated for too. +MAX_LAT_JERK_UP = 2.5 # m/s^3 + LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [15, 13, 10, 5] class LatControlTorque(LatControl): - def __init__(self, CP, FPCP, CI): - super().__init__(CP, CI) + def __init__(self, CP, FPCP, CI, dt): + super().__init__(CP, CI, dt) self.torque_params = FPCP.lateralTuning.torque self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, - k_f=self.torque_params.kf) + k_f=self.torque_params.kf, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg + self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt) + self.requested_lateral_accel_buffer = deque([0.] * self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES , maxlen=self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES) + self.previous_measurement = 0.0 + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) # FrogPilot variables self.nnff = NeuralNetworkFeedforward(CP, self) @@ -51,29 +59,38 @@ class LatControlTorque(LatControl): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, llk, model_data, frogpilot_toggles): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 pid_log.active = False else: - actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - - desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y_NN if frogpilot_toggles.nnff else LOW_SPEED_Y)**2 - setpoint = desired_lateral_accel + low_speed_factor * desired_curvature - measurement = actual_lateral_accel + low_speed_factor * actual_curvature - gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation + delay_frames = int(np.clip(lat_delay / self.dt, 1, self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)) + expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] + # TODO factor out lateral jerk from error to later replace it with delay independent alternative + future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + self.requested_lateral_accel_buffer.append(future_desired_lateral_accel) + gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation + desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay + + measurement = measured_curvature * CS.vEgo ** 2 + measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) + self.previous_measurement = measurement + + low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y_NN if frogpilot_toggles.nnff else LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 + setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel + error = setpoint - measurement + error_lsf = error + low_speed_factor * error if self.nnff_loaded and frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite: pid_log, ff = self.nnff.compute_nnff( - CS, VM, actual_lateral_accel, desired_lateral_accel, gravity_adjusted_lateral_accel, lateral_accel_deadzone, - llk, measurement, model_data, params, pid_log, roll_compensation, setpoint, frogpilot_toggles + CS, VM, measurement, error, future_desired_lateral_accel, gravity_adjusted_future_lateral_accel, + llk, measurement, model_data, params, pid_log, setpoint, frogpilot_toggles ) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 @@ -83,17 +100,19 @@ class LatControlTorque(LatControl): freeze_integrator=freeze_integrator) else: # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(setpoint - measurement) - ff = gravity_adjusted_lateral_accel + pid_log.error = float(error_lsf) + ff = gravity_adjusted_future_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it + ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) + -measurement_rate, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) pid_log.active = True @@ -102,8 +121,8 @@ class LatControlTorque(LatControl): pid_log.d = float(self.pid.d) pid_log.f = float(self.pid.f) pid_log.output = float(-output_torque) # TODO: log lat accel? - pid_log.actualLateralAccel = float(actual_lateral_accel) - pid_log.desiredLateralAccel = float(desired_lateral_accel) + pid_log.actualLateralAccel = float(measurement) + pid_log.desiredLateralAccel = float(setpoint) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 44accf4e2..a076ea2e0 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -26,7 +26,7 @@ class PIDController: self.neg_p_limit = neg_p_limit self.i_unwind_rate = 0.3 / rate - self.i_rate = 1.0 / rate + self.i_dt = 1.0 / rate self.speed = 0.0 self.reset() @@ -61,19 +61,19 @@ class PIDController: def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): self.speed = speed - self.p = float(error) * self.k_p + self.p = self.k_p * float(error) if self.pos_p_limit is not None and self.p > self.pos_p_limit: self.p = self.pos_p_limit elif self.neg_p_limit is not None and self.p < self.neg_p_limit: self.p = self.neg_p_limit - self.f = feedforward * self.k_f - self.d = error_rate * self.k_d + self.d = self.k_d * error_rate + self.f = self.k_f * feedforward if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: if not freeze_integrator: - self.i = self.i + error * self.k_i * self.i_rate + self.i = self.i + self.k_i * self.i_dt * error # Clip i to prevent exceeding control limits control_no_i = self.p + self.d + self.f diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 30b398e7a..3addaba86 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -16,6 +16,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.simple_kalman import KF1D from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpilot_toggles +from openpilot.selfdrive.controls.controlsd import LaneChangeDirection, LaneChangeState # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 0.6 @@ -149,7 +150,15 @@ def laplacian_pdf(x: float, mu: float, b: float): return math.exp(-abs(x-mu)/b) -def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]): +def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], frogpilot_toggles: SimpleNamespace): + if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and frogpilot_toggles.human_lane_changes: + direction = model_data.meta.laneChangeDirection + + if direction == LaneChangeDirection.left: + tracks = {k: v for k, v in tracks.items() if v.yRel > 0} + elif direction == LaneChangeDirection.right: + tracks = {k: v for k, v in tracks.items() if v.yRel < 0} + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): @@ -194,11 +203,11 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool, - frogpilot_toggles: SimpleNamespace, frogpilotPlan: capnp._DynamicStructReader, + frogpilotPlan: capnp._DynamicStructReader, frogpilot_toggles: SimpleNamespace, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > frogpilot_toggles.lead_detection_probability: - track = match_vision_to_track(v_ego, lead_msg, tracks) + track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, frogpilot_toggles) else: track = None @@ -315,8 +324,8 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], sm['carState'].standstill, self.frogpilot_toggles, sm['frogpilotPlan'], low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['carState'].standstill, self.frogpilot_toggles, sm['frogpilotPlan'], low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False) if self.frogpilot_toggles.adjacent_lead_tracking and self.ready: self.frogpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)