From e834b4d9bed65e672c25c6e9ba67d41f2a1c8329 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 27 Apr 2022 01:18:05 -0700 Subject: [PATCH] LatControlTorque: fix deadzone and missing steer saturated warning (#24294) * fix steer saturated alert and deadzone * fix and formatting * update refs * update refs * not needed * remove --- selfdrive/controls/lib/latcontrol_torque.py | 8 ++++---- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2816b2014..862e8b384 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -59,14 +59,14 @@ class LatControlTorque(LatControl): 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 @@ class LatControlTorque(LatControl): 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 index 47d1a8623..4d773a2bf 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -17141a8240bd8ec5defa1de2da937544c52ef0fe \ No newline at end of file +999b5f3bfe249b40758dc62601a6e4118dad82ae \ No newline at end of file