From 6c8bb5673ed9b7dfcc5095e267e8dd55b06eecaf Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Thu, 10 Sep 2026 22:26:20 -0500
Subject: [PATCH] ravbob

---
 .../controls/lib/longitudinal_vehicle_tunes.py    | 15 +++++++++------
 .../controls/tests/test_longitudinal_planner.py   |  9 +++++++++
 2 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py
index bb66a5874c..4c3971fb4e 100644
--- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py
+++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py
@@ -64,10 +64,10 @@ TOYOTA_SIENNA_POST_DEPARTURE_RESTOP_MAX_BRAKE = 0.32
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_EGO_SPEED = 12.0
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_MODEL_PROB = 0.85
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_LATERAL_OFFSET = 1.2
-TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE = 45.0
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE = 30.0
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE = 105.0
-TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_CLOSING_SPEED = 4.0
-TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE = 0.8
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_CLOSING_SPEED = 0.75
+TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE = 0.35
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE = 2.0
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DECEL = 0.5
 TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MIN_SPEED = 5.0
@@ -77,8 +77,8 @@ TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_DISTANCE = 100.0
 TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_TIME = 4.5
 TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_OFFSET = 32.0
 TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_LATERAL_OFFSET = 1.75
-TOYOTA_RAV4_TSS2_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5
-TOYOTA_RAV4_TSS2_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75
+TOYOTA_RAV4_TSS2_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.0
+TOYOTA_RAV4_TSS2_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.25
 TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.70
 TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST = 0.20
 TOYOTA_RAV4_TSS2_LEAD_CREEP_MIN_LEAD_SPEED = 0.15
@@ -427,7 +427,10 @@ def get_toyota_rav4_tss2_early_lead_cap(CP, lead, v_ego, accel_min):
     (TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DISTANCE - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_DISTANCE),
     0.0, 1.0,
   )
-  closing_factor = np.clip((closing_speed - 4.0) / 6.0, 0.0, 1.0)
+  closing_factor = np.clip(
+    (closing_speed - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_CLOSING_SPEED) / 4.0,
+    0.0, 1.0,
+  )
   brake_factor = np.clip(
     (lead_brake - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE) /
     (TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE - TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE),
diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py
index 4230de1420..7e364964ba 100644
--- a/selfdrive/controls/tests/test_longitudinal_planner.py
+++ b/selfdrive/controls/tests/test_longitudinal_planner.py
@@ -3454,6 +3454,15 @@ def test_rav4_tss2_early_lead_cap_starts_a_mild_response():
   assert -0.5 <= cap < 0.0
 
 
+def test_rav4_tss2_early_lead_cap_handles_moderate_closing_before_hard_approach():
+  CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
+  lead = make_lead(status=True, d_rel=35.0, v_lead=16.4, a_lead=-0.5, model_prob=0.99)
+
+  cap = get_toyota_rav4_tss2_early_lead_cap(CP, lead, 17.4, -3.5)
+
+  assert cap == pytest.approx(-0.30, abs=0.03)
+
+
 def test_rav4_tss2_early_lead_cap_does_not_change_other_paths():
   rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
   other = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2022)