From fa7fa6afafebed3a19952229e0e4109e42540731 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 12 Jun 2026 23:09:03 -0500 Subject: [PATCH] Remove Slow Logic atm --- selfdrive/controls/lib/latcontrol_torque.py | 53 ++++----------------- 1 file changed, 8 insertions(+), 45 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 51d180b5a..5d6573b16 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -40,20 +40,13 @@ LP_FILTER_CUTOFF_HZ = 1.2 JERK_LOOKAHEAD_SECONDS = 0.19 JERK_GAIN = 0.22 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 -VERSION = 3 +VERSION = 2 DEBUG_TORQUE_TUNE = False FF_SCALE_BLEND_LAT_ACCEL = 0.05 DEADZONE_BOOST_LAT_ACCEL = 0.15 UNWIND_D_DES_THRESHOLD = -1.0 UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 MIN_LATERAL_CONTROL_SPEED = 0.3 -LOW_SPEED_ANGLE_ASSIST_START_SPEED = 0.25 -LOW_SPEED_ANGLE_ASSIST_FULL_SPEED = 0.5 -LOW_SPEED_ANGLE_ASSIST_FADE_START_SPEED = 2.2 -LOW_SPEED_ANGLE_ASSIST_END_SPEED = 3.2 -LOW_SPEED_ANGLE_ASSIST_ERROR_DEADZONE_DEG = 4.5 -LOW_SPEED_ANGLE_ASSIST_ERROR_RISE_DEG = 12.0 -LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE = 0.36 CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20 CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00 @@ -485,8 +478,8 @@ IONIQ_6_TRANSITION_SPEED = 10.0 IONIQ_6_PHASE_SCALE = 0.10 IONIQ_6_TURN_IN_BOOST_LEFT = 1.64 IONIQ_6_TURN_IN_BOOST_RIGHT = 1.88 -IONIQ_6_UNWIND_TAPER_LEFT = 3.95 -IONIQ_6_UNWIND_TAPER_RIGHT = 8.25 +IONIQ_6_UNWIND_TAPER_LEFT = 3.18 +IONIQ_6_UNWIND_TAPER_RIGHT = 6.55 IONIQ_6_FRICTION_MULT = 0.928 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 @@ -503,7 +496,7 @@ IONIQ_6_CENTER_TAPER_LAT = 0.24 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025 IONIQ_6_CENTER_TAPER_SPEED = 18.0 IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5 -IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.039 +IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.034 IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.09 IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.03 IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5 @@ -519,8 +512,8 @@ IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90 IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06 IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.13 IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 2.20 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.05 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.82 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 3.28 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10 @@ -542,8 +535,8 @@ IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.17 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.74 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.14 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.62 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.94 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90 @@ -649,21 +642,6 @@ def get_friction_threshold(v_ego: float) -> float: return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27])) -def get_low_speed_angle_assist_torque(v_ego: float, steering_angle_error_deg: float) -> float: - speed_factor = np.interp(v_ego, - [LOW_SPEED_ANGLE_ASSIST_START_SPEED, - LOW_SPEED_ANGLE_ASSIST_FULL_SPEED, - LOW_SPEED_ANGLE_ASSIST_FADE_START_SPEED, - LOW_SPEED_ANGLE_ASSIST_END_SPEED], - [0.0, 1.0, 1.0, 0.0]) - error_mag = max(abs(steering_angle_error_deg) - LOW_SPEED_ANGLE_ASSIST_ERROR_DEADZONE_DEG, 0.0) - if error_mag <= 0.0 or speed_factor <= 0.0: - return 0.0 - - error_factor = 1.0 - math.exp(-error_mag / LOW_SPEED_ANGLE_ASSIST_ERROR_RISE_DEG) - return float(LOW_SPEED_ANGLE_ASSIST_MAX_TORQUE * speed_factor * error_factor) - - def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: load_factor = np.clip(trailer_load_kg / TRAILER_LOAD_FULL_ASSIST_KG, 0.0, 1.0) speed_factor = np.interp(v_ego, [TRAILER_LATERAL_MIN_SPEED, TRAILER_LATERAL_FULL_SPEED], [0.0, 1.0]) @@ -2103,8 +2081,6 @@ class LatControlTorque(LatControl): raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) 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) - desired_steering_angle_deg = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + params.angleOffsetDeg - steering_angle_error_deg = desired_steering_angle_deg - CS.steeringAngleDeg gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt @@ -2249,19 +2225,6 @@ class LatControlTorque(LatControl): output_torque *= volt_standard_center_taper elif self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active(): output_torque *= civic_bosch_modified_a_center_taper - - # At crawl speed, desired lateral acceleration collapses with v^2. Use steering-angle - # error to enforce a small torque floor so the wheel starts moving before speed builds. - if not CS.steeringPressed: - low_speed_angle_assist_torque = get_low_speed_angle_assist_torque(CS.vEgo, steering_angle_error_deg) - if low_speed_angle_assist_torque > 0.0: - assist_torque = math.copysign(low_speed_angle_assist_torque, steering_angle_error_deg) - if output_torque * assist_torque < 0.0: - output_torque = assist_torque - else: - output_torque = math.copysign(max(abs(output_torque), low_speed_angle_assist_torque), assist_torque) - - output_torque = float(np.clip(output_torque, -self.steer_max, self.steer_max)) pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i)