|
|
|
@ -42,8 +42,6 @@ class LatControlTorque(LatControl): |
|
|
|
|
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 @@ class LatControlTorque(LatControl): |
|
|
|
|
# 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 |
|
|
|
|