|
|
@ -56,15 +56,15 @@ class LatControlTorque(LatControl): |
|
|
|
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 |
|
|
|
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 |
|
|
|
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature |
|
|
|
measurement = actual_lateral_accel + low_speed_factor * actual_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 |
|
|
|
pid_log.error = error |
|
|
|
|
|
|
|
|
|
|
|
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY |
|
|
|
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY |
|
|
|
# convert friction into lateral accel units for feedforward |
|
|
|
# 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 |
|
|
|
ff += friction_compensation / self.kf |
|
|
|
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5 |
|
|
|
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5 |
|
|
|
output_torque = self.pid.update(error, |
|
|
|
output_torque = self.pid.update(error, |
|
|
|