Skip to content

Commit

Permalink
Revert "Latcontrol torque: integrator need not be reset (commaai#24606)"
Browse files Browse the repository at this point in the history
This reverts commit 5202696.
  • Loading branch information
Casey Francis committed May 23, 2022
1 parent eddde14 commit 9f4a534
Showing 1 changed file with 3 additions and 6 deletions.
9 changes: 3 additions & 6 deletions selfdrive/controls/lib/latcontrol_torque.py
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
if CS.vEgo < MIN_STEER_SPEED or not active:
output_torque = 0.0
pid_log.active = False
self.pid.reset()
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
Expand All @@ -59,14 +60,10 @@ 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 / self.kf
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(error,
feedforward=ff,
override=CS.steeringPressed, feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
freeze_integrator=CS.steeringRateLimited)

friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
output_torque += friction_compensation
Expand Down

0 comments on commit 9f4a534

Please sign in to comment.