diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3fb95371f..92b4251d7 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -28,12 +28,12 @@ INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] LOW_SPEED_X = [0, 10, 20, 30] -LOW_SPEED_Y = [15, 13, 10, 5] +LOW_SPEED_Y = [12, 10.5, 8, 5] MAX_LAT_JERK_UP = 2.5 # m/s^3 LP_FILTER_CUTOFF_HZ = 1.2 JERK_LOOKAHEAD_SECONDS = 0.19 -JERK_GAIN = 0.3 +JERK_GAIN = 0.22 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 VERSION = 2 @@ -84,12 +84,14 @@ class LatControlTorque(LatControl): future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.lat_accel_request_buffer.append(future_desired_lateral_accel) raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) - desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk) + raw_lateral_jerk = np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + desired_lateral_jerk = np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay measurement = measured_curvature * CS.vEgo ** 2 measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) + measurement_rate = np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) self.previous_measurement = measurement low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2