diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2a1b666efd..664e866636 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -77,7 +77,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 * error + error_lsf = error + low_speed_factor / self.torque_params.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)