Fiction compensation should be based on error

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,