diff --git a/opendbc_repo b/opendbc_repo index dfa6807ebf..b59f8bdcca 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit dfa6807ebf9f0edb593189a703b857c834a88a75 +Subproject commit b59f8bdcca8d375b4a5a652d2f2d2ec9cd3503d3 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index fbef5828e6..f2b1e0b669 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -24,6 +24,9 @@ from openpilot.common.pid import PIDController LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [15, 13, 10, 5] +KP = 1.0 +KI = 0.3 +KD = 0.0 class LatControlTorque(LatControl): @@ -32,7 +35,7 @@ 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(self.torque_params.kp, self.torque_params.ki, rate=1/self.dt) + self.pid = PIDController(KP, KI, k_d=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) @@ -76,7 +79,7 @@ class LatControlTorque(LatControl): 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 + error_lsf = error + low_speed_factor / KP * error # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(error_lsf) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 538aa34f2a..dd73ed12f7 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5342f5684c2498a33d6178f3b59efd5e83b73acc \ No newline at end of file +55e82ab6370865a1427ebc1d559921a5354d9cbf \ No newline at end of file