From caeeb30cc36e7c2779d2c07a10c3786eef16a164 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 29 Nov 2025 11:05:32 -0600 Subject: [PATCH] Revert "Test Lagd" This reverts commit b87ba638461b0f6b945410a0541b2e7311f29abc. --- selfdrive/controls/lib/latcontrol_torque.py | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d1a643661..f94051344 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -46,7 +46,6 @@ class LatControlTorque(LatControl): self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len) self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) - self.setpoint_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 @@ -70,18 +69,14 @@ 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] + 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) 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 - - expected_lateral_accel = self.setpoint_filter.update(self.lat_accel_request_buffer[-1]) - - delay_frames = int(np.clip(lat_delay / self.dt, 0, self.lat_accel_request_buffer_len - 1)) - center_idx = max(-1 - delay_frames, -self.lat_accel_request_buffer_len + 2) - lookahead_offset = min(self.lookahead_frames, self.lat_accel_request_buffer_len + center_idx - 1) - lookahead_idx = int(np.clip(center_idx - lookahead_offset, -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) setpoint = expected_lateral_accel measurement = measured_curvature * CS.vEgo ** 2