From 68cef2d360b26ff4f945f99ab3cd8c2c1b78a22e Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 25 Aug 2026 15:49:03 -0500 Subject: [PATCH] stop the kachow --- opendbc_repo/opendbc/car/hyundai/interface.py | 2 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 1 + selfdrive/controls/lib/latcontrol_torque.py | 1 + .../controls/lib/latcontrol_vehicle_tunes.py | 26 +++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 12 +++++++++ 5 files changed, 41 insertions(+), 1 deletion(-) diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index c9aaffdec..a98020e60 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -31,7 +31,7 @@ ENABLE_BUTTONS = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.can # Track when ECU disable happened - used to permanently suppress CAN errors from disabled ECU ECU_DISABLE_TIMESTAMP = 0.0 KONA_NON_SCC_FCA_RADAR_ADDR = 0x602 -KIA_EV9_ACCEL_MAX = 2.5 +KIA_EV9_ACCEL_MAX = 2.2 def apply_platform_longitudinal_params(ret: structs.CarParams) -> None: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index bf3849b2e..953aef96b 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -1653,6 +1653,7 @@ class TestHyundaiFingerprint: assert CP.startAccel == pytest.approx(0.2) assert CP.vEgoStarting == pytest.approx(0.5) assert CP.longitudinalActuatorDelay == pytest.approx(0.3) + assert KIA_EV9_ACCEL_MAX == pytest.approx(2.2) assert CarInterface.get_pid_accel_limits(CP, 0.0, 0.0)[1] == pytest.approx(KIA_EV9_ACCEL_MAX) ioniq_6_cp = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], True, False, False, toggles) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 6a89184ef..dd46774bd 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -408,6 +408,7 @@ class LatControlTorque(LatControl): friction_scale = get_toyota_highlander_tss2_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif toyota_corolla_tss2_active: ff *= get_toyota_corolla_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_toyota_corolla_tss2_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) elif lexus_is_active: ff *= get_lexus_is_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif ioniq_5_active: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 320eec781..197ce7608 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -1116,6 +1116,15 @@ TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT = 0.18 TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08 TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5 TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_GAIN = 0.12 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_ONSET = 12.0 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.0 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF = 25.0 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF_WIDTH = 3.0 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT = 0.24 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.10 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK = 0.25 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK_WIDTH = 0.10 TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE = 0.12 TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION = 0.10 @@ -1662,6 +1671,23 @@ def get_toyota_corolla_tss2_ff_scale(desired_lateral_accel: float, return 1.0 + curve_weight * speed_weight * (boost * turn_in_weight - unwind_reduction * unwind_weight) +def get_toyota_corolla_tss2_friction_threshold(v_ego: float, + desired_lateral_accel: float = 0.0, + desired_lateral_jerk: float = 0.0) -> float: + """Reduce center-only friction chasing on highway-sized model corrections.""" + speed_weight = (_sigmoid((v_ego - TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_ONSET) / + TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH) * + _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF - v_ego) / + TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF_WIDTH)) + center_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / + TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT_WIDTH) + calm_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK - abs(desired_lateral_jerk)) / + TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK_WIDTH) + gain = _flm_vehicle_knob("toyota_corolla_tss2.center_friction_threshold_gain", + TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_GAIN) + return get_standard_friction_threshold(v_ego) * (1.0 + gain * speed_weight * center_weight * calm_weight) + + def get_toyota_corolla_tss2_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: """Taper only near-center crawl-speed torque during manual handoff.""" center_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 0d048adcf..9578cd2cb 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -117,6 +117,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_toyota_highlander_tss2_friction_threshold, get_toyota_highlander_tss2_output_taper_scale, get_toyota_corolla_tss2_center_output_scale, + get_toyota_corolla_tss2_friction_threshold, get_toyota_corolla_tss2_ff_scale, get_lexus_is_ff_scale, get_camry_ff_scale, @@ -444,6 +445,17 @@ class TestLatControl: assert 0.65 <= crawl_center < cruise_center <= 1.0 assert crawl_curve > crawl_center + def test_toyota_corolla_tss2_friction_threshold_targets_center_highway_band(self): + base = get_standard_friction_threshold(16.0) + center = get_toyota_corolla_tss2_friction_threshold(16.0, 0.0, 0.0) + curve = get_toyota_corolla_tss2_friction_threshold(16.0, 0.8, 0.8) + slow = get_toyota_corolla_tss2_friction_threshold(5.0, 0.0, 0.0) + fast = get_toyota_corolla_tss2_friction_threshold(30.0, 0.0, 0.0) + assert center > base + assert curve < center + assert slow < center + assert fast < center + def test_flm_standard_friction_curve_override(self): base = get_standard_friction_threshold(10.0) overrides = normalize_flm_overrides({