From e7e593cdb2be10cf72a41003b775e702421490f6 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 10 Nov 2025 23:07:02 -0600 Subject: [PATCH] Revert "Upstream Lateral" This reverts commit 97f87366053d182ebc86f0270472801ca43135a6. --- selfdrive/car/card.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 24 ++++++++++---------- selfdrive/ui/ui | Bin 74666392 -> 74666392 bytes 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 636f28956..bf43487da 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -30,7 +30,7 @@ class Car: def __init__(self, CI=None): self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState', 'liveCalibration', 'onroadEvents', 'frogpilotOnroadEvents', 'frogpilotPlan']) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'liveCalibration', 'onroadEvents', 'frogpilotOnroadEvents', 'frogpilotPlan']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'frogpilotCarState']) self.can_rcv_cum_timeout_counter = 0 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index eb45d0191..fa6bf76b3 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -28,10 +28,8 @@ 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] LP_FILTER_CUTOFF_HZ = 1.2 -JERK_LOOKAHEAD_SECONDS = 0.19 -JERK_GAIN = 0.3 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 -VERSION = 1 +VERSION = 0 class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): @@ -42,12 +40,10 @@ class LatControlTorque(LatControl): self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) 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.jerk_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 * LP_FILTER_CUTOFF_HZ), self.dt) self.previous_measurement = 0.0 + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -73,18 +69,17 @@ class LatControlTorque(LatControl): 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] - lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2)) - raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt) - desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk) + # 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) gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation - setpoint = expected_lateral_accel + 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 + setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel error = setpoint - measurement # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly @@ -92,10 +87,15 @@ class LatControlTorque(LatControl): 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(error + JERK_GAIN * desired_lateral_jerk, 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, -measurement_rate, CS.vEgo, ff, freeze_integrator) + output_lataccel = self.pid.update(pid_log.error, + -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 diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index bdddb00000a04f0b8c0c315c8a1b62ec25989e44..7cc0d72dc6ac53fb54a3cefbea43279d26481665 100755 GIT binary patch delta 6149 zcmYk+1yEFN0EJ<8Su8?I5J3!7#O_w?0J}R7#ljB6z{bYJ?(XhZY_Y|}?hfqw&kL9F zcjkTW_4sjjWoLHwvRI$lYICsJHD;T)e`M&>(y6_M{+OFH!CXABVdTg#b7Yv=q_|ou zi?UHER7#afrB-Q_t+G@0%0Z=7j><_nD;MRe(kVCPuF@+Hl|gwbFO^Ywt4u1h%A&F= zAC*mIS2=ctnSHMYc+QCqcj|3rWnOD#va(Wc7ejPmwaf~s>~2O_VV|*0rTE+XlEBYkyr6j0dMJIwom#S+AF?Y^*MSGaGBTmZixLPMDjtI>pWE zQ9JSf8Sb=P2lg93W_s&q{8)8e6GvTaLPL$!O)n8=JS=@|sif6$gbp?~o?vA;k+_EW z#OrKRV|9zw??J!d#MRW(c!FI-#iYaG&U)V_y*K?aS=sxqG7dk}-*XtNm9_1^N0`SN ztK0d(NvmIA;sVL@>97~Zk0#n zRryqYRX`O~g;ZfxL={!VRB=^8l~kovX;ns*RppewDz7T2imH;Ttg5K0s+y{Z*FGzG|Qvsz$1@YNDE|W~#Yrp<1d|sZkgv0V+%lRN-on8mxw>p(;WRQ^VB=HByaIqtzG{ zsm7{tYP_1DCaOtlvWiku)KoQ1O;WDh3 zj;R=RT%AxS)hTsaol$4iITfqUs|)I)x}+|vE9$DcrsC9fbwk}$x72NQN5!kV>Ylo< z9;k=vk$SA2sHf_gdahomm+F;zt=_1&>YaM8KB$lClS)vZ)fe?ueN*4n5A{?1Qoq$7 z^>>1AT~m|-GgyNKY#;@sgjA3k(ts`4fju}tT5tp>a0VA}g>>Ks?vNflAOm=U7i0u) z$OM@o3uFZ!$OhRV2lzrx@Pk~C8}dM2$OrkM02G8mP#B6pQ78t*p#+qKQcxPoKv^gU z{!kt&Kt-qom7xk$g=$b8YCr%4LQSXzwV@6qejfF>s}BvJAvA)<&;*)7GiVMipe3|| z*3bspLOTe8_7Dsmpd*ApC+G}apeuBPQ0NXlpeOW#-p~j7LOW@IU?XgT&9DWw!Zz3rJ76d5g59tO_QF2c4+r2N z9D>7e1dhTnh=Jp90#3pyI1OjuES!T_I1d-#B3y#Ya0RZyHHd@ja070_Ew~MLARg|* zJ-81K;2}JM$M6K6!ZUadFW@D-g4gf{-oiV04$N-+;1sTB` zGC^j@0$ITavO#vp0lttE{2&+PhCGlL@O%u)2#ugIG=Zkj44Oj=XbG*L zHMD`Y&<=v2Jp@As=m;Uu2|7a;=nCB+6uLtX=n1``H}rwN&=2~<00@JD5DtT2FbsjA z5COwrIE;XiFbYP)7>IAU1egeuU@}C(6qpLrU^>iznGg-LU^dKwxiAmr!va_c zi(oMK3vR<5h=;py z5AMSQcnFW+F+72%@C=^A3wQ~y;5EE~x9|?$!w2{XpCAD~!x#7p-{3p^fS>RSe#0O5 zYu4AFwQfvc25Yc@4WxjSkP1>m8n6XBum=Z73y$Cf&fo&BkPh6y9nymbWB^a_f{fq| znIJP{fvn&I*&sXQ0AI)nevk`tLmtQr`5-?OfPzp63PTYn3dNu}lz@^@3Q9v6C=2Dl zAId`os0fvyGE{-8Pz|a>4G4fhs0p>8Hq?Q{&!Zl9^`QYYghtR9nm|)%2F;-bw1igB z8rncxXa_;i9)h6*bc7J-1f8J^bcJpZ3f-Xx^n_l}8~Q+B=m-5_0EEFn2!}y17>2-5 zh=5@*97e!M7zLwY3`D|M7zg8F0!)NSFd3p?3QUD*Fdb&VOo)bAFdOE;T$l&*VF4_J zMX(r_z*1NS%V7nqgjKK_*1%d=2kT)2Y=lj)8MeSy*aq8S2keAhup9QkUf2iw;Q$oACz)3g-r{N5og>w)K=ivfegiCN4uE15e261p5Zoo~r1-Ic2#KT>< z2lwFtJcLK^7@ojWcm~hm1-yh;@EYF0TX+ZW;RAexPmlng;R}3)Z}1&{z)$!Ezu^!3 zwbs|4MK>legEd&d22wyuNCl}O4cLMm*nmNC$4<4(Y)IGJq#|K}PU~ zOpqC}KvwX9Y>*vtfG^|(Kgb2SArIt*e2^asKtU)3g`o%(ggXYizT0$#m s4Q-$;w1XgM55dp@Izk9^g3izdxFm^Dp zIH{@bU*aCEZDmXy6LcTar>5b?4mOdsSgebPY0AHi#kO9n|1CBtlD@II^}nfEbo#~@ zhNVsKhNVs*{kn~dut@udsfo*TS7V3sx-{-zB#T|Pj`}w?PrnG4Bdv{TqKS!lfz&;; zb_e%`jWxCKNT0sQ@o#G0A$_{<{{Kx6r%#g>|C>5k1gDSR%`wJ%qTK#-Q$>}#@=(Q8 zaaBTvZ|aauPUgDs*Z$t5 zPx-3`DnK<8t0`)#nx>|!8EU4QrDm%+YOab^^HiLguNJ6XO8Ff~jQ|DEJx}YwqOX{+^qOPiI>bgo)H`GmaOWjs?)LnH?-B%A( zl6t5fsmJPxda9nO=jw%esa~nq>WzA<-l_NMgZijGsbrO+KC3V4tNNzCs~_s8`lWuW zKkDy9xBpD1Dlh|cumDTQ02#pwGC^jr1{<)2EMNy&Asb`|dvJgp;0R9O3@(rpazSp$ z1Fn!4@06{Op<*1Ak}$0niW{LF)5p%vlp?3eBK7w1Ae-3R*)OXbbJ2 zJ#>JM&%jDfK*4#qI+h9BFfSs@lcEcXn3;SR{#KQqN z2#4S>9D$>7435JII0>iVG@OC6a1PEx0$hNLa0xEM6}Sr5;5sD24Y&!n;5OWWyKoQg z!vjcyhwumfE{FoY>*x7!2xoBBRGLGxIj+G1-T&) zxI$jY2l=4@6of)h7>a-!6a{zifMQS_NOftn2lc@Z{GkB^KtpH*sn4S^XHB3fG=t{Q0$M^VXbo+kEwqF7 z&;dF^CkTWf=nTQo1-e2Abc62D1A0O)2!$}{4dKuS`a(bG4+CHz41x$43`1Zj41?h? z0wQ4~jDpcH2FAiT7!Oep4KXkQCc-3`3{zk#OoQn#17^Z3m<@AaF2uq-h=cjC02aa` zSPV;GDJ+BKumV=XDp(C`U@feJ^{@dp!Y0@ZTVN|}gYB>bcET>$4SQfO?1TLf4+r2N z9D>7e1dhTnI1VS^B%FfNa0br8IXDjqZ~-pDCAbV%;3`~$>yQXH;3nLH+i(Z&!acYT z4#J~iY2$NtkOo6E|4W`2km!v@$0n_x3+fvvC&w!;qC3At6F7ql1e!uKXbvr)CA5Op&<5H