From 14f5dabe6e5768a3324a2314ab245e646503827f Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 20 May 2022 13:50:47 -0700 Subject: [PATCH] Latcontrol torque: integrator need not be reset (#24606) slow integrators need not be reset --- selfdrive/controls/lib/latcontrol_torque.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d4650f02568cfb..64c9880831eed5 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -42,8 +42,6 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi if CS.vEgo < MIN_STEER_SPEED or not active: output_torque = 0.0 pid_log.active = False - if not active: - self.pid.reset() else: if self.use_steering_angle: actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) @@ -62,10 +60,11 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi # 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 / self.kf + freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(error, - override=CS.steeringPressed, feedforward=ff, + feedforward=ff, speed=CS.vEgo, - freeze_integrator=CS.steeringRateLimited) + freeze_integrator=freeze_integrator) pid_log.active = True pid_log.p = self.pid.p