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

…ommaai#24294) * fix steer saturated alert and deadzone * fix and formatting * update refs * update refs * not needed * remove
spektor56 · May 7, 2022 · 53e477b · 53e477b
1 parent 5fde416
commit 53e477b
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 5 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
@@ -75,5 +75,5 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
       pid_log.output = -output_torque
       pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS)
 
-    #TODO left is positive in this convention
+    # TODO left is positive in this convention
     return -output_torque, 0.0, pid_log
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-17141a8240bd8ec5defa1de2da937544c52ef0fe
+999b5f3bfe249b40758dc62601a6e4118dad82ae