diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 47f0b69f2bda76..1078af30b9e79c 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -41,10 +41,6 @@ def __init__(self, CP, CI): self.friction = CP.lateralTuning.torque.friction self.kf = CP.lateralTuning.torque.kf - def reset(self): - super().reset() - self.pid.reset() - def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() @@ -59,9 +55,9 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 - + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature - desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + #desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature @@ -71,7 +67,7 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY # convert friction into lateral accel units for feedforward - friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) + friction_compensation = interp(error, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) ff += friction_compensation / self.kf freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(error,