From 2d945b2fd5ad666954a40cb4140752bf4395ee56 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 27 Jun 2022 15:25:47 -0700 Subject: [PATCH] Torque control: higher low speed gains and better steering angle deadzone logic (#24980) * Try no friction and no deadzone * Learn fromd ata * update refs old-commit-hash: b3226d505b9260ec8565acfda7138601a76039a6 --- selfdrive/controls/lib/latcontrol_torque.py | 6 +++--- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 014c3480b8..46caa41a90 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -56,15 +56,15 @@ class LatControlTorque(LatControl): lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - low_speed_factor = interp(CS.vEgo, [0, 15], [500, 0]) + low_speed_factor = interp(CS.vEgo, [0, 10, 20], [500, 500, 200]) setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature - error = apply_deadzone(setpoint - measurement, lateral_accel_deadzone) + error = setpoint - measurement 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(error, [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-self.friction, self.friction]) + friction_compensation = interp(apply_deadzone(error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_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, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 292c41b978..1402284b96 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b55de9fdb2416e63ab554c95233a78219d8d3db9 \ No newline at end of file +a16ca1082cd493f6cea5252eaaba9f8c6574334a