diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 12714082a61eb9..f72ffc4b88ea7b 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -19,8 +19,7 @@ # move it at all, this is compensated for too. -LOW_SPEED_FACTOR = 200 -JERK_THRESHOLD = 0.2 +FRICTION_THRESHOLD = 0.2 def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0): @@ -66,14 +65,16 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature - measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature + + low_speed_factor = interp(CS.vEgo, [0, 15], [500, 0]) + 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) 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, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) + friction_compensation = interp(error, [-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, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 61bbd5cdd13400..b8976e5f389733 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fe2da24194e3def1823681cc18e7879f24edfc6e +1d66eed104dbc124c4e5679f5dddf40197b86ce9