From 533fb46221612371d1ae946da84b1fb6f9de246b Mon Sep 17 00:00:00 2001 From: felsager Date: Thu, 13 Nov 2025 16:13:12 -0800 Subject: [PATCH 1/4] lower kp and no derivative term --- opendbc_repo | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 35 +++++++++------------ selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 17 insertions(+), 22 deletions(-) diff --git a/opendbc_repo b/opendbc_repo index b59f8bdcca8d37..75aee73540855e 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit b59f8bdcca8d375b4a5a652d2f2d2ec9cd3503d3 +Subproject commit 75aee73540855e704bbae33102f8f53c0978b0de diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 443fd1851c52c7..0ba38736db4f01 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -20,15 +20,17 @@ # Additionally, there is friction in the steering wheel that needs # to be overcome to move it at all, this is compensated for too. -KP = 1.0 -KI = 0.3 -KD = 0.0 +KP = 0.8 +KI = 0.15 + 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 = 0 +VERSION = 1 class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): @@ -36,13 +38,13 @@ def __init__(self, CP, CI, dt): self.torque_params = CP.lateralTuning.torque.as_builder() 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([INTERP_SPEEDS, KP_INTERP], 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.previous_measurement = 0.0 - self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + 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) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -68,17 +70,15 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat 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] - # TODO factor out lateral jerk from error to later replace it with delay independent alternative + 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 - desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay + setpoint = expected_lateral_accel 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 @@ -86,15 +86,10 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat ff = gravity_adjusted_future_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset - # 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) + ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, 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, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) + output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) pid_log.active = True diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index cdd4301fcf90ab..04bfa57e5a27f1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b508f43fb0481bce0859c9b6ab4f45ee690b8dab \ No newline at end of file +9c19ec84096dba2ef8b12f52e6fb3d0b7a44ccd3 \ No newline at end of file From e415382ed98f8d8e1ba41aaada23e00365c60eae Mon Sep 17 00:00:00 2001 From: felsager Date: Mon, 24 Nov 2025 17:43:15 -0800 Subject: [PATCH 2/4] update refs --- opendbc_repo | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc_repo b/opendbc_repo index 75aee73540855e..6171d1a976b632 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 75aee73540855e704bbae33102f8f53c0978b0de +Subproject commit 6171d1a976b632c4804e90e74a78370532a2f297 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 04bfa57e5a27f1..4a58e321fc575a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -9c19ec84096dba2ef8b12f52e6fb3d0b7a44ccd3 \ No newline at end of file +e0ad86508edb61b3eaa1b84662c515d2c3368295 \ No newline at end of file From 0deec51cb2059b889f31bd8c9e32d794bffce748 Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 25 Nov 2025 09:45:34 -0800 Subject: [PATCH 3/4] . From b2faea550f48176aa122a54c2fdcec09dd0f575e Mon Sep 17 00:00:00 2001 From: felsager Date: Tue, 25 Nov 2025 10:18:33 -0800 Subject: [PATCH 4/4] .