yeet

selfdrive/controls/lib/latcontrol_pid.py

@@ -46,10 +46,12 @@ def get_civic_bosch_modified_pid_output_alpha(desired_angle_deg: float, desired_
   onset = min(max((abs_angle - 3.0) / 5.0, 0.0), 1.0)
   cutoff = min(max((22.0 - abs_angle) / 8.0, 0.0), 1.0)
   band_weight = onset * cutoff
+  large_turn_weight = min(max((abs_angle - 16.0) / 6.0, 0.0), 1.0)
   transition_weight = min(abs(desired_angle_delta_deg) / 0.35, 1.0)
   sign_change_weight = 1.0 if (output_torque * prev_output_torque) < 0.0 else 0.0
 
   smoothing = band_weight * (0.36 + (0.22 * speed_weight) + (0.12 * transition_weight) + (0.12 * sign_change_weight))
+  smoothing *= 1.0 - (0.50 * large_turn_weight)
   return min(max(1.0 - smoothing, 0.18), 1.0)
 
 

selfdrive/controls/lib/latcontrol_torque.py

@@ -226,8 +226,8 @@ IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.18
 IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.34
 IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.86
 IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 4.55
-IONIQ_6_CENTER_TAPER_MAX = 0.046
-IONIQ_6_CENTER_TAPER_LAT = 0.18
+IONIQ_6_CENTER_TAPER_MAX = 0.056
+IONIQ_6_CENTER_TAPER_LAT = 0.22
 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02
 IONIQ_6_CENTER_TAPER_SPEED = 18.0
 IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -332,6 +332,48 @@ def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead()
   assert cem.slow_lead_detected
 
 
+def test_tracked_highway_mild_closing_lead_does_not_trigger_raw_slow_lead():
+  v_ego = 65 * CV.MPH_TO_MS
+  cem = make_cem(
+    model_length=v_ego * 5.0,
+    tracking_lead=True,
+    lead_status=True,
+    lead_d_rel=46.0,
+    lead_v_lead=v_ego - 1.4,
+    lead_model_prob=1.0,
+  )
+  toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
+
+  cem.slow_lead_filter.x = 0.0
+  cem.slow_lead_detected = False
+  cem.starpilot_planner.starpilot_following.slower_lead = False
+  for _ in range(12):
+    cem.slow_lead(toggles, v_ego)
+
+  assert not cem.slow_lead_detected
+
+
+def test_tracked_following_slower_lead_still_triggers_slow_lead():
+  v_ego = 65 * CV.MPH_TO_MS
+  cem = make_cem(
+    model_length=v_ego * 5.0,
+    tracking_lead=True,
+    lead_status=True,
+    lead_d_rel=36.0,
+    lead_v_lead=v_ego - 3.0,
+    lead_model_prob=1.0,
+  )
+  toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
+
+  cem.slow_lead_filter.x = 0.0
+  cem.slow_lead_detected = False
+  cem.starpilot_planner.starpilot_following.slower_lead = True
+  for _ in range(24):
+    cem.slow_lead(toggles, v_ego)
+
+  assert cem.slow_lead_detected
+
+
 def test_slow_lead_does_not_linger_at_crawl_when_stopped_lead_disabled():
   v_ego = 1.5
   cem = make_cem(

selfdrive/controls/tests/test_latcontrol.py

@@ -279,7 +279,7 @@ class TestLatControl:
     assert get_ioniq_6_center_taper_scale(0.0, 10.0) < get_ioniq_6_center_taper_scale(0.0, 30.0)
     assert get_ioniq_6_center_taper_scale(0.0, 30.0) < get_ioniq_6_center_taper_scale(0.2, 30.0)
     assert get_ioniq_6_center_taper_scale(0.0, 12.0) < get_ioniq_6_center_taper_scale(0.25, 12.0)
-    assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 1.3e-2
+    assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 4.2e-2
 
   def test_kia_ev6_ff_scale_curve(self):
     assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0