diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f2b1e0b669..31d982859b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -4,7 +4,6 @@ from collections import deque from cereal import log from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction -from opendbc.car.tests.test_lateral_limits import MAX_LAT_JERK_UP from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED @@ -28,6 +27,8 @@ KP = 1.0 KI = 0.3 KD = 0.0 +LP_FILTER_CUTOFF_HZ = 1.2 +LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): @@ -35,13 +36,13 @@ class LatControlTorque(LatControl): 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(KP, KI, k_d=KD, rate=1/self.dt) + self.pid = PIDController(KP, KI, KD, 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.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 * (MAX_LAT_JERK_UP - 0.5)), self.dt) + 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 @@ -64,11 +65,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.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)) - expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames] + 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 future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - self.requested_lateral_accel_buffer.append(future_desired_lateral_accel) + 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