From 884fcb8315aecbfec61094bbd44ac4194ae5fc42 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 18 Nov 2025 15:43:52 -0600 Subject: [PATCH] New Lateral Changes --- cereal/car.capnp | 6 +- cereal/gen/cpp/car.capnp.c++ | 73 +++++++++--------- cereal/gen/cpp/car.capnp.h | 36 ++++----- cereal/libcereal_shared.so | Bin 723128 -> 723128 bytes frogpilot/common/frogpilot_variables.py | 3 +- .../lib/neural_network_feedforward.py | 3 +- selfdrive/car/interfaces.py | 4 + selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 43 +++++------ 9 files changed, 82 insertions(+), 88 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 02971fda4..fffecb1a2 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -530,14 +530,14 @@ struct CarParams { struct LateralTorqueTuning { useSteeringAngle @0 :Bool; + kp @1 :Float32; + ki @2 :Float32; + kd @8 : Float32; friction @3 :Float32; steeringAngleDeadzoneDeg @5 :Float32; latAccelFactor @6 :Float32; latAccelOffset @7 :Float32; - kpDEPRECATED @1 :Float32; - kiDEPRECATED @2 :Float32; kfDEPRECATED @4 :Float32; - kdDEPRECATED @8 : Float32; } struct LongitudinalPIDTuning { diff --git a/cereal/gen/cpp/car.capnp.c++ b/cereal/gen/cpp/car.capnp.c++ index 8a48d82dc..f4b6e4f8d 100644 --- a/cereal/gen/cpp/car.capnp.c++ +++ b/cereal/gen/cpp/car.capnp.c++ @@ -5262,7 +5262,7 @@ 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<166> b_80366e0e804ecc1d = { +static const ::capnp::_::AlignedData<163> b_80366e0e804ecc1d = { { 0, 0, 0, 0, 5, 0, 6, 0, 29, 204, 78, 128, 14, 110, 54, 128, 20, 0, 0, 0, 1, 0, 5, 0, @@ -5289,62 +5289,62 @@ static const ::capnp::_::AlignedData<166> b_80366e0e804ecc1d = { 0, 0, 0, 0, 0, 0, 0, 0, 240, 0, 0, 0, 3, 0, 1, 0, 252, 0, 0, 0, 2, 0, 1, 0, - 5, 0, 0, 0, 1, 0, 0, 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, - 249, 0, 0, 0, 106, 0, 0, 0, + 249, 0, 0, 0, 26, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 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, + 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, 2, 0, 0, 0, - 0, 0, 1, 0, 2, 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, 106, 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, - 1, 0, 0, 0, 3, 0, 0, 0, - 0, 0, 1, 0, 3, 0, 0, 0, + 8, 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, 74, 0, 0, 0, + 9, 1, 0, 0, 106, 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, - 7, 0, 0, 0, 4, 0, 0, 0, - 0, 0, 1, 0, 4, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 17, 1, 0, 0, 106, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 16, 1, 0, 0, 3, 0, 1, 0, - 28, 1, 0, 0, 2, 0, 1, 0, - 2, 0, 0, 0, 5, 0, 0, 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, - 25, 1, 0, 0, 202, 0, 0, 0, + 17, 1, 0, 0, 202, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 24, 1, 0, 0, 3, 0, 1, 0, + 36, 1, 0, 0, 2, 0, 1, 0, + 6, 0, 0, 0, 6, 0, 0, 0, + 0, 0, 1, 0, 6, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 33, 1, 0, 0, 122, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 1, 0, 0, 3, 0, 1, 0, 44, 1, 0, 0, 2, 0, 1, 0, - 3, 0, 0, 0, 6, 0, 0, 0, - 0, 0, 1, 0, 6, 0, 0, 0, + 7, 0, 0, 0, 7, 0, 0, 0, + 0, 0, 1, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 41, 1, 0, 0, 122, 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, - 4, 0, 0, 0, 7, 0, 0, 0, - 0, 0, 1, 0, 7, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 49, 1, 0, 0, 122, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, - 48, 1, 0, 0, 3, 0, 1, 0, - 60, 1, 0, 0, 2, 0, 1, 0, - 8, 0, 0, 0, 8, 0, 0, 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, - 57, 1, 0, 0, 106, 0, 0, 0, + 49, 1, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 56, 1, 0, 0, 3, 0, 1, 0, - 68, 1, 0, 0, 2, 0, 1, 0, + 44, 1, 0, 0, 3, 0, 1, 0, + 56, 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, @@ -5355,8 +5355,7 @@ static const ::capnp::_::AlignedData<166> b_80366e0e804ecc1d = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 107, 112, 68, 69, 80, 82, 69, 67, - 65, 84, 69, 68, 0, 0, 0, 0, + 107, 112, 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, @@ -5364,8 +5363,7 @@ static const ::capnp::_::AlignedData<166> b_80366e0e804ecc1d = { 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, 105, 68, 69, 80, 82, 69, 67, - 65, 84, 69, 68, 0, 0, 0, 0, + 107, 105, 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, @@ -5420,8 +5418,7 @@ static const ::capnp::_::AlignedData<166> b_80366e0e804ecc1d = { 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, 68, 69, 80, 82, 69, 67, - 65, 84, 69, 68, 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, @@ -5435,7 +5432,7 @@ static const ::capnp::_::AlignedData<166> b_80366e0e804ecc1d = { 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, 166, nullptr, m_80366e0e804ecc1d, + 0x80366e0e804ecc1d, b_80366e0e804ecc1d.words, 163, nullptr, m_80366e0e804ecc1d, 0, 9, i_80366e0e804ecc1d, nullptr, nullptr, { &s_80366e0e804ecc1d, nullptr, nullptr, 0, 0, nullptr }, false }; #endif // !CAPNP_LITE diff --git a/cereal/gen/cpp/car.capnp.h b/cereal/gen/cpp/car.capnp.h index 20a2e4aa1..a4afdba6c 100644 --- a/cereal/gen/cpp/car.capnp.h +++ b/cereal/gen/cpp/car.capnp.h @@ -3134,9 +3134,9 @@ public: inline bool getUseSteeringAngle() const; - inline float getKpDEPRECATED() const; + inline float getKp() const; - inline float getKiDEPRECATED() const; + inline float getKi() const; inline float getFriction() const; @@ -3148,7 +3148,7 @@ public: inline float getLatAccelOffset() const; - inline float getKdDEPRECATED() const; + inline float getKd() const; private: ::capnp::_::StructReader _reader; @@ -3181,11 +3181,11 @@ public: inline bool getUseSteeringAngle(); inline void setUseSteeringAngle(bool value); - inline float getKpDEPRECATED(); - inline void setKpDEPRECATED(float value); + inline float getKp(); + inline void setKp(float value); - inline float getKiDEPRECATED(); - inline void setKiDEPRECATED(float value); + inline float getKi(); + inline void setKi(float value); inline float getFriction(); inline void setFriction(float value); @@ -3202,8 +3202,8 @@ public: inline float getLatAccelOffset(); inline void setLatAccelOffset(float value); - inline float getKdDEPRECATED(); - inline void setKdDEPRECATED(float value); + inline float getKd(); + inline void setKd(float value); private: ::capnp::_::StructBuilder _builder; @@ -7754,30 +7754,30 @@ inline void CarParams::LateralTorqueTuning::Builder::setUseSteeringAngle(bool va ::capnp::bounded<0>() * ::capnp::ELEMENTS, value); } -inline float CarParams::LateralTorqueTuning::Reader::getKpDEPRECATED() const { +inline float CarParams::LateralTorqueTuning::Reader::getKp() const { return _reader.getDataField( ::capnp::bounded<1>() * ::capnp::ELEMENTS); } -inline float CarParams::LateralTorqueTuning::Builder::getKpDEPRECATED() { +inline float CarParams::LateralTorqueTuning::Builder::getKp() { return _builder.getDataField( ::capnp::bounded<1>() * ::capnp::ELEMENTS); } -inline void CarParams::LateralTorqueTuning::Builder::setKpDEPRECATED(float value) { +inline void CarParams::LateralTorqueTuning::Builder::setKp(float value) { _builder.setDataField( ::capnp::bounded<1>() * ::capnp::ELEMENTS, value); } -inline float CarParams::LateralTorqueTuning::Reader::getKiDEPRECATED() const { +inline float CarParams::LateralTorqueTuning::Reader::getKi() const { return _reader.getDataField( ::capnp::bounded<2>() * ::capnp::ELEMENTS); } -inline float CarParams::LateralTorqueTuning::Builder::getKiDEPRECATED() { +inline float CarParams::LateralTorqueTuning::Builder::getKi() { return _builder.getDataField( ::capnp::bounded<2>() * ::capnp::ELEMENTS); } -inline void CarParams::LateralTorqueTuning::Builder::setKiDEPRECATED(float value) { +inline void CarParams::LateralTorqueTuning::Builder::setKi(float value) { _builder.setDataField( ::capnp::bounded<2>() * ::capnp::ELEMENTS, value); } @@ -7852,16 +7852,16 @@ inline void CarParams::LateralTorqueTuning::Builder::setLatAccelOffset(float val ::capnp::bounded<7>() * ::capnp::ELEMENTS, value); } -inline float CarParams::LateralTorqueTuning::Reader::getKdDEPRECATED() const { +inline float CarParams::LateralTorqueTuning::Reader::getKd() const { return _reader.getDataField( ::capnp::bounded<8>() * ::capnp::ELEMENTS); } -inline float CarParams::LateralTorqueTuning::Builder::getKdDEPRECATED() { +inline float CarParams::LateralTorqueTuning::Builder::getKd() { return _builder.getDataField( ::capnp::bounded<8>() * ::capnp::ELEMENTS); } -inline void CarParams::LateralTorqueTuning::Builder::setKdDEPRECATED(float value) { +inline void CarParams::LateralTorqueTuning::Builder::setKd(float value) { _builder.setDataField( ::capnp::bounded<8>() * ::capnp::ELEMENTS, value); } diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so index bbe1027bda60d881e9803d399c3853db9aa770b1..0cf8130fea5477123f96446d8fa657582e83b3c6 100755 GIT binary patch delta 16628 zcmeHPd3+RAw(cTHFKnj6(%lJ5f+%52(u82c0453~QQ##$g&+cnqfS(o1eeJuVB&%q z1|*??6Bkx-USxzJI*&w0)(03DFi~*g$kJgHA_69?0gb%xRNceSHNW}u{rCF!>(kWz zzH`pKx2kU4`_&CK<%4R<2bJZzf}89%{shY(-Qfq4HIH=d3Ow_!(*5rZpH6{9DUscRoLIcI>c}kq1t`icnzT)ad0vYu+;>R9Mggeb*13%;vwS_ya@ZOS#ul{*1~MZz*4> z$DQ{0yziUQiY=&D&jz|o`Bqxcm3j%#iVsvx28Gt7Laze)n&h>htMx|C`=RMuYC&;X zo=Mc!>uF~^zMU#UtA!KL3xNjqm=R(Jhh{&juLD}NSH-uWMSA@i&s|L_@)iqvS~6P7 zTlbmKk`h95weHi0oK5gIE4LgA9QC&8w$@T!tCyek_-gA+wAO+S)pr3+Rlc!_p()Gs z7NBJZP2UU)I$n?a(c_ygc`azJo((kjYn9i67U(5F7f9Zu(7d&J70}uvDz61ytTzI! zkh~VOMt3)RTs4gws>RTo%-;*8}yQ zP`NDVhq~6{@$HtpLqhX(4fAnCDGrcd|NGO zt13@T4$WJm$DQ+NHTv*#i9Y8>GjfH66Q!4&!_9xeM4K(BlskupM(&}ae>ZFcA383VWHVK>wch>?M<}Uf>!EzKx73o4V=GUrf+!deucdJ^o~N zdXa@xtTzHJ_L?EqTF^+{eaWM5x#)DaT96_=?Gi@A7}GCycxdWay%1=;(F*jBTN8c7ij!sGlQD+3+Iw5ueFrdj5Mp#U@g~6e)9OPDlc1V zXvS#03TW+E(>LFOV&eEyqBcTz|Ln=kf5*H98!VirdraS^e^YKt3(dGvFZ>y&%O%G|3oPhb-QSjo%Oww-gDQT|!U^c*Km!k$ z5uA61#>Z0(Xx20p--2f8Ep4966Do3r1#Om$mh#F6&1kLGay|Q(M6FCO`32L>T$7VO z##*1MUp!h+|M-_g-*UyNv~X7H?rXTg9yTM`#)d|~4R*~lq(b=?SkTsd)2sAf%JoIp z5`6oV+ZGFaSg%J)?;O+FIW9D%cZH^TeCL#Jkp(SV;nzG`*@{dp(Kjg1j8J3Y1Xh$I zLVl5nMy7|x&tI_%=osZ2u%P2sv;Zyro#|U5)5884Zl#atAx<>dBHJbJx509a`1rhhq_K)qiQr2@3(ezqb z#OUfR&qX|=(Xy6^aW5B@L{t}**uNd<3zl8IHU_neEWdg!2ftbPor&KHdTNusU$^Ce zlW@qNNPGw3yM?MY*}G_mY40Zc%@H;@gp+o_o=%Ym?1BF`|CfEB7ib-;w`{Y=KCHb- z^?e+_af|5o%Il+(em-!=|Kw2S&5k*4QQO{2zAro>edwEB`WVGgBC|z3mKT3}uWqZF zme5^&9kbn{uVr8Od#|az?pqP0kNa|z3UdbX+~uRk2WIDe<)zG9P%IkT>76^e{*cgp zX)Jw=VqflSYMsUx3qA*?=pDV22CmGd>|5a_dRzHd^;4=}cV7-tJ&L15XgiR4xZ&pe zzFyP(Cf(D|F*`~Ww(OBF49Ne-tJj(~&=)AzYAG$=aT|Vq7w?!eT*S5da}O>(-{`K8 zW4UI{n%Hb2v+;2|e8*EPOa2-z@*}|ZV@gOwV|91S3 z*9LvHu|}k_5vsN}rEZBv#@DJF-zcgY>_~Tu210lx_f?4i_QtDAuhdW#*p!ikDujWy(WHrZl|Hnh z@1eg=-5pPUKZ0(#AtjvBY+m9DkkCHW|HVnLTWWhHI$a@NYpS@N_Kd?{8tp8>%q;3hGgtsygQQGC7{1+~p@yR>dgb6Ye$L zMHl+$m&fj$IBUr3a{Pe!soz+OJFaHVF4E953s2qq?xtf)<#?*aJ;~x8Fx-7fapsgz z*d3$dT~U2Xw(8$&8y|uIu;h)&>pVYJn|2BWV9P-*YmV) z6g$%4>|Zd@Y}-neIZ`P;#mZG~O^jgZ$!;PH&FP!{8ddDL|qKab5U()~HKZ-3Ze+ z>vs6!!0;NSd{dR8BZ`ptUNTRQ2tQdD0wcygdwi`N_mM}93Ugfo!ZA;tqp4-IBRve- zk5TR1RPI701Z_~hSxVFyq2vor%>zF1{zKXkfwzGbyYK|{c}@w1Y&G{@xr zqWR1n+<3Ts=KG(@aqf*={CfkH7<47`RDN>M|KPI|cZ%$Di-o?Mv^yM=uj@X7PHbC| z!_*yEwgP#zp1>7+1-h)Q#-JV+rE66w2F*!|?8#AKK_U8-Z zhu`14yF|+OD4-Oo^YsRrLbgZXcnYc4VfSHo^)%lDO`_{{`Ttb|KKN}7Da1mtg z+mQYBpr`l0CCAAo>$(+Xh70)4-D-A;C^Eu(GrXYfS1&HPQc3>1(UBi0aYU}T56x~D z38rr3fZ}H!EnbMZ71$BmnN1~su$vQ^;Uc;CR;|i(o$h`?j>k$i^(zt#A98m=dco4Gsc(i}b|a=tE|_^}&8Ml9HUTXX4Tf99 z0Ut8FhKt6st71UUv-{jH$+44pH+)2M;UXS5OAYtxq=}9vmc$I}<_fFnV=g_Mf5!tR z3C0tRe~h5*-#C0JUxN_1*(5Ar!XJfj1W2kv-e)8T^EV&3Nx;Gv2;os6 zsR}7)qz1;w`YIjfYP|u%PDOAW0U-g#AlyrlM-Y3e&lQ%T_OZ-o!UiFH3uLE4Y8VN^ z7`=6prn773Xe94vRx9Bs2$Nt8f*VMNLLOzL493WsLhryeCICXMA{=9a{g@(@Q4j>} z_3Eof919%71L${WkY@rI?OHGrCByi8!ecm)XoZYpWH!vsUO;XKl0#MBIl9GsgMSWE zr2R~~2vZva(qU@+&f$wV4uY4iF*k@an4!2!&w*PF^B+CHZKT8#Xzxi7LH61YV$A8?v8KtzxE10(Oj z{GK~65~unO<+G2GvoK$?j|YfoHSWtmhQst>pWCQs6RI|vuadX;sf}Ta2|HlCu^=p? zY!I|p>2dyft@@=VM<+W5B>k+Tul8nj%~{6|`%T^mTkHY1&35y*Xs+$e>V+2_x89`L zXj8b;8!fWVSCN`6;uo0Vw<7K+SJ-$u#<=J?T>6ovK& zVD>YT0;KsbbOgC$M8NqgGyVZ{h8a7+SV)m}r!PgM9Di33rob4Dy8?o2hnmUAazU;F zSxM8`Rdz%jXF~gK%5`WvSC~jVJyB>MD@Y2E)$|Tr(f#}E&g2vkb)vh2cLz)i9*yDY zVHizR%kE=jK1@6#6MzIL!GTUWstBJl;XKSmOd!LHK@d&H?g5ejW3=cmK$tXa0bTc*B3#! zF6Q`v#KRcKOF&X-ywmB65n1PDCj1%Ze>ir3J6})5Af(8KrZy(r60O}T4{gs))Qm{1FX>}9&jNS_#0t-Bnqu*KBFg=)!mrUGF03Yebp zaLopxhO$6N+3>b`d!1lH=N_7E6caXpAUm7lP-xGDxr>pFK;mgNyNVD*OxPi=XF!mR zOP&9S1@bXbsq@1RK~Fl zWeBbKJrk;6b}->82%-@kVx$emxLlv?#HalzyFwi@I^gcUAUpwcj9s4vLFA%^jBJK6 zI-qlBS6Cel>V#@RFEU(zWhxg!G&6T%x4TXqi{36L<9k zVJ^%J&M^@LQHEA9vJJ+Fy%|U>EnrvKMARW3gk+fK*!90b5LIXrBMV>**RGMSuq65n zt}y|X^)n`1fLX(?F-%x_lQTI+grNETfxiV~3}2IhkD-Y-A-70E=a~>QK#d<)L69v( z2`IEr667Z!_tP72#X52;QVbJ8Xuv@5?twAxse9e7Fxf)%Fe6J~j8XM|AkF=$ccjys zB5Ke#%m^E#s<#vj*)Y@{h4#@f$GBlDfE3WoNL=T|v^Em;FB4H{Fn9xCt}$;!6gpRJ z7@Ek)(=fdu+JwIcWG$VAD{gY9+ql<9y3vz$D$rBdPG(;PGl=QEyWsp$0SGA#itqyy zx+H2gKVN_Mfgq~StthllgfY(2d?3eZJ6zi}2k2|Jb4a^Wf1*xNc#g`}o@q&NcnM}E z=lYyOQEC*%6;W|I`#~56W1O~bU2!S@iIyTniU>H^3xSIYW)p`m#yoH|BY1-S!A zG#!pI+I?UxF8nr3UX&{H1p`-MiWtD0El7hCoPKRN%}Bu1 zY^9=YPXp})Oa;@P;QPEBc#Q0!8G8rtSulGUzk-KFKJ7`sC?pcgC1CbmcWSopn6MX< zq{HM+MBjL6Abw&*Sg8TU9)vLxy@VN28O=zju@ND7;qNW^U zf@7>|>M#(-P$Op=M?W!7bd+_V*guAu#epV-V|bv%B%Gf|^6bXY>KO;Z?J&rMKG^2@&u3ynh)0)NGFDC6B8oSHCqi6R)T;^FYh5qgD!^#SvLC+J8!6d?T~A`UCW>;hh0nm`6mM`B;m7vh8YAe zzK^&(K6z&yZ!R)zJ#$SM!5OY;F=r3~Q6zruchP_Q@N7O3sGYYPyMuz1TcAd&S>epuQ8gd^4ER$FX zzC^)El*X_;8{$>&BML62LWbqVaI=C3KV$}vrF9I;`@afxQO#FyF4Z$EFY{BC_kINz zkro6j^UosHu$~W_!SN!9VOgHaNtmVJGRk9EHX6h_%|73D-MgG&nHO)73*ZqmcoFSl zSl+0_b~3%EDY$}K7?$@$K~?ij3dZa0M!>R}wL%4NQ*ah#Gb}5MO65J_H)ietl`tGU zZJy8u)$UaauBonSbdh%bk7+uZRkO`{%yiGEynn!bp;|c$yiLIYDrZ<0JnfZtr^iii zFYN-HDN79u&cSTLKmCLeHvTai3*cYl&6a zCd@P|BfF&~+f%As*1w$2A9pxk~Bq? z%OCz+zeH5|Mrz?~B9KV9O$yGXxRZcIu@bmV!AX?Oux#Xzq!S9w%3uxvI8(MvNbXe% zmJL19#J#Fq_Pt29Gpam;8abN?Y?5u%Tr*oNxtjpXVo0JcQSeGi1Dq-I`dHO*M-(h; z1#ydb!i*|gb~-EmU{zjB>p0tS%M~lha8O3v_n1#h7GA6)5L8Tn3Q+9a<%cd7;-RjeG^bsGI4ub3OuvnSzqW)69Y9IMXZ zBq>*0nbnp7+X7V{q_{JvlPpVQesDm+m6Xk}yzz_gV%DbDGiJ61Dq&b&aaO1&-E0M8 z>cX(RM3)J`b_HWEHp8-B*`iw7`B^h~DY?(0rSjHVS~^|9^6oNT%{msUa(RI&le*2S z99ziFy8h-{uGSTDaz@WLvmK_qA5oc7HMx&fm3c+M#NVb7t@tsf1x!v`NEeE4YHH z7?ve&ch#`%3eKlShK)DHfaKQiTx z!P=+`Ek5U(rv+Yiop)8c&bPnGt!0rfyhEbv`u|&izcJ`=n=4=acLw>@<>4Lrh5s*Z C$pEVW delta 16656 zcmeHPX;@TOwyn!hgHtG{qQn7=i8w(aV$f&;CI&F-gLLB1PGFmEHEO^NO($v~BW*{8 zT+JqfifQ!`pVH8I9f++Fea4s=2^eD=;{X{*bLZh!sr{d@I&wQt?C z*53P^bEk9GJy26Ju%={SakeY4#ctzIpk&Stcc$98xz&ehYddH4uG(DsVpY3|vwCh! z>bh~&FZJWLy>~|2{GB(qFx+rXzujqUL&J=XEmyBPndbGqY@&NDs84tQkk-w%T>171 z_O&h77yj^oH}INrw!i_s;fHw5r)z!>tk)m!O@2c$?+6Z*tmpXSwS2t<%zXV_{|s+n zt;wvvGng6BV@}0)bFMeh%)Y^>bG<(IR9rXP2IXvlZ5#Bzo{G~N^`=uE*sG^zcq6x% z)Ie--q)0vQw8xuWZlclsg3(#}I-sQ$CYoQ?Hi*t$)vqP-;Po^qoI?GMs-?@dpIQm-0wU`D-dytfhR79&^Uy z^?hJQE3lwGJqze25u7$b#auJR7fV z)l<)UyoXhUfQ6H(=K;;!ZAOS56rBCCz7A;dM=HJrE!G>(ddAq^HPK278m7tvmhy;? zO};NKI9J5S`rvbM2~OoU$pZIRm$Jsk&B(DYa=?y@ACskex zdQjI|Jl-RccW`iCpPtadH?)kFcyFWP%(ifv^%6t~oH66qThM^Mvn4*kw#h^T7BoVY zrz8gFt=D7Dd$f9e*!g&GwBnRnIQ{j)^SJmgnUR|NJE-zzOS!G98O@m-oZZ${fBsUO7O7WV z!ZSoU=UYgndK1vnNHav41&!6+mvQPV-+%=z)>DBNN149S!-CT%>v=#^B(DX{)z<;d z?Wyuw&~&{4Xr|=-7gW3A(b9GAm3Z$Y#Yr9>oPDaEbH(HJ_cr4fTTs7V0yJCs)>zPm z`c9ygeNErUyMrTF>Md8|wHNi6fG1<&S56H*Md5S>94iM zC4_8NcPR^OSLMm~T2r>PdNjZ8`Z+$Mjp7toI2}~^UQ2oX2(v1Ot>t>*&mM0_skENv?lE8`yWE>1!Jm9QlZ@UBl@z$wYH4XpNq5Egq*!4mf92{6-5WQ!fFUIn|8d zye~LDo@qd5PgC(N=xn{^nkVDBidk-t?`Fj*w{R+S_YGWNkD3u|qk|*h0=wZET&a9>Eoj6X)2rxT%Jro; z;=IR|TcriQs5c;`?{U-F`9N?=-zrV>c-!QdXubt4UzMPFwDMIMTD&Rt76p<29l zfJ&HkPz5QoumZG`;lQhAki8c85VZi#a4uIl#s=q*@^nl2QI&0yrTiPp3PUzuu^Fz$ z0{f^i3|011mBCV%ZtS?=q?4!#ur`UVg6z##tl1XUB1#QM%_BCLscJ2-lpnT~7pQQ4 zOL^HV>%ujBL+}m7H>`SHc=+SLr`Wf&cGP`wc!V~WruDH$QMo&`9liRd)-GJrY__?y z?5U6_O{2nG%|o@}_Au)13++yg%$ny5X-9!@d$blp+A`2SrL#V4LaD(Q;-T5YL!+rN zCoGI+1D)o$8l_KSWEjLLLZtY>u(5E_*Kg z8O``fwJ(M^U&$^Eug)&Ce>cFpx48Aj{j30CI|y_>ddvELr<2NXtyb@ntWuCpiqzxhAxgMrnR+wIYhY8$Dcx8rwi zQR9}sb&v0tx>5g=OBuI29(Rij_g>;7VR30g{_LYqP~1`Uw~ljjV(;zQWwmWNjp^f9 z;1=O6>*A4~Q+htKDnOs~;V7l%!0Ndx$G;hnm2=2P8F!#q#J4lGJ39Xu*L6iQeS%_d z{+nt7hZBQ8b(8gWkrM{Io=;hK!b^m>l0yxXtKV{82~Y!yyNCi;mvZ#*?T>u5*4U?} z=k1+nc3;P&jv~Qjjd-E|>~G(=VcS3l`Z^Y9jg%VexC@&<#5x`yChA+m`I##&G`UM< zUru7PMmC%1aJ(Oa?<|Vx=SUkS0^HeODqg>u@_eoAXNjNMiWJAYEPLh}?*G)_6s_!7 zzb($XqeaoA^>=vO`AS&Aoxt!aFudHNyS;Q^<>zhEJa5!fA=p|G#SC&x^oqW=sVV8L zqlwp+%Kl6A#{rJ0aK4|dG2@7)HhI$LiBnd;`7Z9C1CUk3G_DgjwZdeJidOggU4_4$ zaOLw#(guPp@|m#TH`tnw@__ae`7Df^fKUEF^JcvDmX_Uy^23H#Khesv9-E=9?K^j$ z?8h3;|1_74rrs^$SJ^zPj>p<+83Z9h!Vp#r%U7K6EUDkFz3Xu%j0?se?fi>52~t zpBBwR3s4o)aG^6zR4SoCoid}$e4#s;JtY#;y_HWqJahNRl?7DD{Edpw_24dm)@1TUR+@OZ{~e!wk+(OW zw4unp$H>0(9hKcJ`qI-=dYzr@%>73#6@o^Qcd4U(RN{%4^wVT*>6otB`*TUV3;9Jw z;{1F%T^7EGN&nFrI;q_kwlDou2)rMK$W2|JkPt4=t+mQwg(0F5+N(16XLVHQZhA#{E;| zq6Znq$EhTq7jMbISL9N&2Sg!y>+f3@MP98~Bm34HV(~3Kz*SsnxQ7-EuCvr=}g2{Q=5dHqF#9(lN1%h(a0r zrgmLecX^|+uNQ`O(po9&9@GW8kkQcEdr%hl}Zf~b%^W2UptkgIMPFgo35Gn z!qq?AY{p3Cnxe!V5r@RJ18FH3Er0(DBz+`JB&Y zeN#huqi{(yswQyDxPQ+WO>m3u^Z3xD$KJTo>r5uKfQ_dF$8?MC^UR9X@L`m=BLyY69L*1fIT3z{jtIo^qw0a2@!&iDK?|qWFGOt&`FsM}!?t(iY!~Mv^X~=uG?D4+$%-ov1MOsbQVgslCWK9H<93KTXfo z!o1~A1~+Ecf*oxq?4Ej|oJt-*V?^NLJWR}_tPC_pl$|Ws!L2FB9$i*I?liDP(%~HJ zG2LW~mXkF0{ufFozJ8>PN|-Gg4zqdu2ki#2y`s^i42n2fdFa1Cll`tnrTKvy?H0Y} z>kf07eK(YIgj3uZH4JY!v%ct)|p)*`i$`Y9F}|S?iZI`t)+wq(UAv~R3c~GL9^RM zkf|Thzu=iU1utVn1$Ou$W>d_c?B?KQmCeHZ-SEj1bLV}zcHbmQ9fuZ)BEv1> zfQt;TVWPO~EbZUp+#dH!vhQTxEgw-}xQOqaWrlmTZ@lA)Wl_VrxI$}sn^O?y5jF{ySlp}d0(?yamPWJ0Amn314&oN97c*^jI7DD8Lm-xgRoZ-PB6iKLJ@Ro1VLL< z-FnQi#4+q{^t-c>#{#(9wct*a2$RqQ9wUHsQ^*61EP$!)31k$IT&nus(Ix6QehyTm z{Y<(9vo{K)6V&v*!yEoR2tK;O+;4FN(-n8cd2p*?{-Zm%-%5n!?CzL6*~ExJIo6LBx`eneY=#N-wzHOI09*KY-)pqt8z|y3BbDKM$*5 z`4_=0gBi!%g+N3^d5@8=VU}6g27~~u{Q*@wg`W{k=F!N!3~oNmR!-9lL?oF%G4dYF zAGz~7;ZToKK6@BB2lEyCbORzXjr$6aVKBYfXE^onqiU%jG^(Xk#DpC%k`ufT=?a}iCm0tgXRI6elQ6$!ywnvso7~~RbCra#i}BMiPcXjL z1=aPwaA!g{5pQ1T2>icb4F7Ac&;nW>?(_~773V4-drT+IR?Gfznk%$OMIM4e`&5|y zj3fbZ{*8_yca*G^`a3heg*nTN9bl}X2)ompB+^boXAmaC7>&CMf~Y(57+EPuE07AB z&aR^Ke8Yq`U6kvP5Lc*3Jv~rpA1z1{koRdbT+#h|?9Rj_5qG+E1#c8g6dsLXX`wi) zRm&b>WHC%EBjbQ%Qk(;wa#<1fG2sHtB@7|M3P2ET$L;|V2V=D8zk%4^rNs_sWOrE< zwTKz(VbVGH-@%CZn5rDk!BL{(+!G1LZ(*Khh8FG$?N8T{K1tM^kC<>4#^~1aI=xY%@4Ui;zIob?ho@ohq1c2<93S_JSbmoLU*_9i^%@#^DNGPu*RpmMnY91!gaW=@ARp zED-7`6NIE~s$nOY(4o6#8_9(AAjqnx7!=wwV8$@A2}la9VONobhzUEy^(+Xo=;;6> ztuS-h^#Twdo#ASULX_SEgj^USYqHZ7dW53e7^l+kHqJz-vE=mB~MyzZhACG-MeGE6lSmb!54H&II)XCwrgHf<3wn(k?fszV$y0?vsBzXZnJ z4}51ETw-eF(t1WdgE0<9Dv%-cDqN#P7wR7a!b32|b)5^sD5?bknT~L#G1Q?QGRexL znV96+m%=>9nYy>beL$@`TFuBuFh-qs0NF@W;Tk0ik2>8B!rd@y*>w*H`BcoY4K)bu z_#+dlV0JK}6$H_RzGUPYjB&a?*`5#kadw3;WOTrTeL#2u<^;Pw3xX&`FEdgBV{|}= z4tPk=!1kyXG$OWe78`A0i*FRf+$3BD6~%y z20`T9{D~}3>8hN{{ZkFf-$bChuyAFSy1#SBgo zBXppu-U={e`A}CB+V6om!3|plq=@E4;5;v-vIx|_Tx6j^;Pr>O!Mx!e(Yb2*(0E3k zhUo>_ChR>RHFOTHxX7Ju<60l#Mo;=xpr_(MUkx*m>AgDP_)#thNzIDzBNIBsYqkVF z{~iHBbe}s>Xde$_9HqrTTIoZ$hB$}OS8nIvkdV#Pz9XKave0Lm4-PNE%;Q|2b0|vb zi2I5NIUN!}7z$$?wl1AX%G{G1T;kSWoqs^Vr&!W-94*_8!jB&br2Eq|)1Od8TYbWEd#C1lm%bJ|uC4s&I zW(wZzhCPhIrRXy6F!Cvk5$IVU0a^stD3N9&hk}p-W84qUfpDEFK?refsovMwc`Z__ zrB4Pq6B0#yp%e_AcECJ_nOxXi7`!M|<^Th&F!>B%%odDe zBupvOp5W`e1bCFJs2M#9_VUlNYiXJOn4mxOrm^fg(xMPfY}ege9we|82oIe8=PD;ljw91 z(qYas;nyI@(v~TVya3aUdol+|GmT3?@?9!<0~4;o++bH7gjgyB0jnC`VqTr%J)Z&I zCKw}719-Lcc>*daN=V`u5T?KwCyfn*q=VFBFd~R1@-7n^V8(XjR)BDbvOoxNE~j~e zae9c_@#_acd>+OiE(Y-%S_>kM{s)6`W%%hV$MvZ-AFquCeJjj=a@@-poJ^vaMAS?a zjr+!d@EA-lCM02SvWOl}ME8l7@c|Q>VEjx-!;nJMjEjtPAFq&fAZ4#?PjpsqPjn4z zON9rWURwHen~v3;?{&R8UCX3(t6d(gfHz&L>sGtEg=#CSPnEe|NkHB64!G{slBw!| zE6ppDs{$q6%*$l;uB|OV?k`;UD-cTk!j%?+xBW}MaK(9LeitxK7bs>~Cfi31U$|yK zZ%a6cw7%+$gD!^#VK(bacHU6^=9jJy?OHZXKI&TW&p#(%Aqk)TGt3}(=Y7oOx%v7! z)|_P8dSsh0f-_v-Vh%o%mG^uFmr@SHGNuYB@9hdMrV@r_xZJF~+vk|UBWb4*T>U{y zveJ~MDY%$g0B6YClmZ2BQLt#~#j5;ERW6T)2p5`(&ox!x_sj12o{tGbSxX;!9) z3K^D%S+t6}TETe7%COIGcEn!geOAGZ)Wop7$gWqpNB+hPUQg~5fMpOX!IvpGol+T= zXG5&YeN4fnl*h2VCT>>npjl?{WLn3ty!9(pC)HvF=Tig2@=`xVdGA+nF=>r}W&D}1 z8rI`cGdNxaF)Y(lISBI=TuwO*%YuVwr`hM*Z+e$7EaT!zIRSoa1}~i*n^X?ssPHOe$np{#4_zYWHdd z*H>3Hxk$VDhc@lZs@dkxHQfs+=Uce1Q8Q_#$$W#M2 zXX7(X!7`^-b+Wyr%4J$9*|w;1EQ|RL*<>KC4ub8Pf>%-&;Ecg))Do?-jhkmyM%GSC zwx?9N%zry5{%TcTPF0+(LN#67&MLT>ni!S=rBr6*d^1~ra(@q4CTMXg_%a1grBsGR z<`M5>3a+C(hNIPBRNz4i%;1-49m5M1jQhFqS*+mqsexh91O(o%;KQVy1T51fIcPl| zH-jIg1cqgJC^et2;H8wquncacTed6s2$cZNkO5sgRkilHW^h>wElHD9x%^Sk%~M2` z@1hpYCIX3s+oIrniunPsC{_YrQ*b(EF)WKWBN-wOY=g#R}d;4Gha78EN`{1vip*3b068y;blYPnt=`Py)loY860o zdd^qyB+6m@{wXs!-sm$db9D(mO~HGpg<;Y1q?flScpJr> z1}sZYq+6~jIGeH<7O_!!bKKKr?pag_I72i>$oy*c^{BfJKbWpw-JV5Ih7VS!o z@L~mTqlO<{X<9jX&tTXjuRUv3gO4j#F6}&n{*YJ9ht;zu?hj@Td5IjYj^P9;R|}if zssY;)RUV+2v#66yOJsadr{G%3Vp!hz#db1l)AJcKTQe0hEU!3A)st?4f-!VqSYD#b z0N_IfV`VnOGGD1wE$#5E8N7_#=g?AlYb`CEu3&k08LLJeFROBSfhvQ#3RR9}W9MA| z@NQP~3OP9UEH<-Uq@15nnKCuFk5-jguHflZ!mvz8RZiP+1>>>GuuNLyyop<42CtwN zh8L;e0xwbU5{hXCoT;iNr&XPT@%EKrS(7ihdp>6df13&!mPwm5Y=MF+sfuBl;&xRH z`%u9J)WopyrWlZ1`W*_);8V%n0$66~l6$&>{gleEOzPy$P@&*YDUV^9HV;Pv{#-9G zgKwsF49k2;x@CN!31?FS!!oCqZds$?qokb&Eb~CA+BpSprG)dC(%`s)g$?FLo#@r` wu0>krE3ON!YS)D}x4E_1Z@4ERv4e%ESZSQgwsDE`(P+bz%wr|+~0>V`Zod5s; diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index 8cf071973..e4dc77c2c 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -19,7 +19,6 @@ from openpilot.selfdrive.car.mock.interface import CarInterface from openpilot.selfdrive.car.mock.values import CAR as MOCK from openpilot.selfdrive.car.toyota.values import ToyotaFlags, ToyotaFrogPilotFlags from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN -from openpilot.selfdrive.controls.lib.latcontrol_torque import KP from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.system.hardware import HARDWARE from openpilot.system.hardware.power_monitoring import VBATT_PAUSE_CHARGING @@ -572,7 +571,7 @@ class FrogPilotVariables: startAccel = CP.startAccel stopAccel = CP.stopAccel steerActuatorDelay = CP.steerActuatorDelay - steerKp = CP.lateralTuning.pid.kp if CP.lateralTuning.which() == "pid" else KP + steerKp = CP.lateralTuning.torque.kp steerRatio = CP.steerRatio toggle.stoppingDecelRate = CP.stoppingDecelRate taco_hacks_allowed = CP.safetyConfigs[0].safetyModel == SafetyModel.hyundaiCanfd diff --git a/frogpilot/controls/lib/neural_network_feedforward.py b/frogpilot/controls/lib/neural_network_feedforward.py index a849d3cdb..e7495d32c 100644 --- a/frogpilot/controls/lib/neural_network_feedforward.py +++ b/frogpilot/controls/lib/neural_network_feedforward.py @@ -13,7 +13,6 @@ from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.controls.lib.latcontrol import LatControl -from openpilot.selfdrive.controls.lib.latcontrol_torque import KD, KI, KP from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.modeld.constants import ModelConstants @@ -162,7 +161,7 @@ class LatControlNNFF(LatControl): self.nnff_loaded = self.lat_torque_nn_model is not None self.torque_params = CP.lateralTuning.torque - self.pid = PIDController(KP, KI, k_d=KD, + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.use_steering_angle = self.torque_params.useSteeringAngle diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index f652104e1..f32ca9ec1 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -283,6 +283,7 @@ class CarInterfaceBase(ABC): ret.vEgoStopping = 0.5 ret.vEgoStarting = 0.5 ret.stoppingControl = True + ret.longitudinalTuning.kfDEPRECATED = 1. ret.longitudinalTuning.kpBP = [0.] ret.longitudinalTuning.kpV = [0.] ret.longitudinalTuning.kiBP = [0.] @@ -298,6 +299,9 @@ class CarInterfaceBase(ABC): tune.init('torque') tune.torque.useSteeringAngle = use_steering_angle + tune.torque.kp = 0.6 + 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 90e3f23e9..a812f780c 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -773,7 +773,7 @@ class Controls: if self.frogpilot_toggles.conditional_experimental_mode or self.frogpilot_toggles.slc_fallback_experimental_mode: self.experimental_mode = self.sm['frogpilotPlan'].experimentalMode - if hasattr(self.LaC, "pid"): + if hasattr(self.LaC, "pid") and self.CP.lateralTuning.which() != "pid": self.LaC.pid._k_p = self.frogpilot_toggles.steerKp # Update FrogPilot variables diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index fa6bf76b3..e9842c2a2 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -16,20 +16,15 @@ from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_G # wheel slip, or to speed. # This controller applies torque to achieve desired lateral -# accelerations. To compensate for the low speed effects the -# proportional gain is increased at low speeds by the PID controller. -# Additionally, there is friction in the steering wheel that needs -# to be overcome to move it at all, this is compensated for too. +# accelerations. To compensate for the low speed effects we +# use a LOW_SPEED_FACTOR in the error. Additionally, there is +# friction in the steering wheel that needs to be overcome to +# move it at all, this is compensated for too. -KP = 0.6 -KI = 0.3 -KD = 0.0 -INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] -KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] +LOW_SPEED_X = [0, 10, 20, 30] +LOW_SPEED_Y = [15, 13, 10, 5] -LP_FILTER_CUTOFF_HZ = 1.2 -LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 -VERSION = 0 +MAX_LAT_JERK_UP = 2.5 # m/s^3 class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): @@ -37,13 +32,13 @@ class LatControlTorque(LatControl): self.torque_params = CP.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([INTERP_SPEEDS, KP_INTERP], KI, KD, rate=1/self.dt) + self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) - self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len) + 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 * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -57,7 +52,6 @@ class LatControlTorque(LatControl): 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() - pid_log.version = VERSION if not active: output_torque = 0.0 pid_log.active = False @@ -67,11 +61,11 @@ class LatControlTorque(LatControl): curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len)) - expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] + 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.lat_accel_request_buffer.append(future_desired_lateral_accel) + 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 @@ -79,11 +73,13 @@ class LatControlTorque(LatControl): 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) / max(CS.vEgo, MIN_SPEED)) ** 2 setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel error = setpoint - measurement + error_lsf = error + low_speed_factor / self.torque_params.kp * error # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(error) + 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 @@ -103,11 +99,10 @@ class LatControlTorque(LatControl): pid_log.i = float(self.pid.i) 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.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(measurement) pid_log.desiredLateralAccel = float(setpoint) - pid_log.desiredLateralJerk = float(desired_lateral_jerk) 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 - return -output_torque, 0.0, pid_log + return -output_torque, 0.0, pid_log \ No newline at end of file