LatControlTorque: fix deadzone and missing steer saturated warning (c…

…ommaai#24294)
cydia2020 · Apr 27, 2022 · bcf714d · bcf714d
1 parent cb90d86
commit bcf714d
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
@@ -59,14 +59,14 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
       pid_log.error = error
 
       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])
+      ff += friction_compensation / CP.lateralTuning.torque.kf
       output_torque = self.pid.update(error,
                                       override=CS.steeringPressed, feedforward=ff,
                                       speed=CS.vEgo,
                                       freeze_integrator=CS.steeringRateLimited)
 
-      friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
-      output_torque += friction_compensation
-
       pid_log.active = True
       pid_log.p = self.pid.p
       pid_log.i = self.pid.i