diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index deb98e5e023620..6bcda113ca6840 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -66,15 +66,15 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi 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,