The Triple Dipper

selfdrive/controls/lib/latcontrol_torque.py

@@ -547,6 +547,12 @@ IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 4.5
 IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8
 IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.10
 IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05
+IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST = 0.10
+IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED = 18.0
+IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5
+IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06
+IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22
+IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035
 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
@@ -1742,7 +1748,17 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo
                                         IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH)
     crawl_turn_in_scale = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT,
                                               IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight
-  return (1.0 + crawl_turn_in_scale + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
+  high_speed_right_turn_in_scale = 0.0
+  if desired_lateral_accel < 0.0 and desired_lateral_accel * desired_lateral_jerk > 0.0:
+    high_speed_weight = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED) /
+                                         IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH)
+    high_speed_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START) /
+                                            IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH)
+    high_speed_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END - abs_lateral_accel) /
+                                             IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH)
+    high_speed_right_turn_in_scale = IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST * high_speed_weight * high_speed_lat_onset * high_speed_lat_cutoff
+  return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale +
+          (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
 
 
 def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:

selfdrive/controls/lib/longitudinal_planner.py

@@ -211,6 +211,9 @@ LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0
 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5
 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0
 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15
+RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN = 0.75
+RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP = 0.04
+RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR = 0.75
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED = 12.0
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_MODEL_PROB = 0.95
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA = 0.35
@@ -1367,8 +1370,6 @@ class LongitudinalPlanner:
 
     lead_delta = float(lead.vLead) - float(v_ego)
     min_lead_delta = LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_LEAD_DELTA if low_speed_follow_window else LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA
-    if lead_delta < min_lead_delta:
-      return None
 
     desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
     if low_speed_follow_window:
@@ -1378,6 +1379,23 @@ class LongitudinalPlanner:
       gap_buffer = max(LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN,
                        LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN * float(v_ego))
     gap_error = float(lead.dRel) - desired_gap
+
+    radar_matched_follow_active = (
+      lead_radar and
+      tracking_lead_active and
+      self.lead_is_matched_follow_window(lead, v_ego, t_follow)
+    )
+    if radar_matched_follow_active and gap_error > (gap_buffer - RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN):
+      return None
+
+    if (radar_matched_follow_active and current_source == "cruise" and
+        gap_error <= RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR and
+        lead_delta < min_lead_delta):
+      return RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP
+
+    if lead_delta < min_lead_delta:
+      return None
+
     if gap_error > gap_buffer:
       return None
 

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -2447,6 +2447,46 @@ def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelera
   assert cap is None
 
 
+def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave():
+  v_ego = 22.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(
+    status=True, d_rel=41.558, v_lead=22.3, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0,
+  )
+
+  cap = planner.get_lead_catchup_accel_cap(
+    lead,
+    v_ego,
+    1.45,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45)
+  assert cap is None
+
+
+def test_route_8bc6_radar_matched_follow_catchup_cap_holds_small_cap_for_slower_lead_on_cruise():
+  v_ego = 22.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(
+    status=True, d_rel=39.108, v_lead=21.2, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0,
+  )
+
+  cap = planner.get_lead_catchup_accel_cap(
+    lead,
+    v_ego,
+    1.45,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45)
+  assert cap == pytest.approx(0.04, abs=1e-6)
+
+
 def test_post_departure_pullaway_bypass_does_not_skip_when_lead_brakes_again():
   v_ego = 19.03
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -1,3 +1,5 @@
+import datetime
+
 import pytest
 
 from openpilot.common.constants import CV
@@ -206,6 +208,51 @@ def test_standstill_seeded_force_stop_hold_requires_clear_window_before_release(
   assert not vcruise.forcing_stop
 
 
+def test_standstill_seeded_force_stop_hold_accepts_datetime_now_without_crashing():
+  planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False)
+  sm = make_sm(standstill=True)
+  toggles = make_toggles()
+  base = datetime.datetime(2026, 6, 18, tzinfo=datetime.timezone.utc)
+
+  first = vcruise.update(
+    controls_enabled=True,
+    now=base,
+    time_validated=True,
+    v_cruise=20.0,
+    v_ego=0.0,
+    sm=sm,
+    starpilot_toggles=toggles,
+  )
+  assert first == pytest.approx(0.0)
+  assert vcruise.standstill_force_stop_hold
+
+  planner.starpilot_cem.stop_light_detected = False
+  second = vcruise.update(
+    controls_enabled=True,
+    now=base + datetime.timedelta(seconds=0.4),
+    time_validated=True,
+    v_cruise=20.0,
+    v_ego=0.0,
+    sm=sm,
+    starpilot_toggles=toggles,
+  )
+  assert second == pytest.approx(0.0)
+  assert vcruise.standstill_force_stop_hold
+
+  released = vcruise.update(
+    controls_enabled=True,
+    now=base + datetime.timedelta(seconds=1.2),
+    time_validated=True,
+    v_cruise=20.0,
+    v_ego=0.0,
+    sm=sm,
+    starpilot_toggles=toggles,
+  )
+  assert released == pytest.approx(20.0)
+  assert not vcruise.standstill_force_stop_hold
+  assert not vcruise.forcing_stop
+
+
 def test_nav_turn_speed_control_default_off():
   _, vcruise = make_vcruise(nav_state={
     "valid": True,

starpilot/controls/lib/starpilot_vcruise.py

@@ -100,6 +100,11 @@ class StarPilotVCruise:
 
     self._nav_instruction_state = {}
 
+  @staticmethod
+  def _elapsed_seconds(now, since):
+    delta = now - since
+    return delta.total_seconds() if hasattr(delta, "total_seconds") else float(delta)
+
   @staticmethod
   def _nav_maneuver_target_speed(maneuver_type, maneuver_modifier):
     maneuver_type = str(maneuver_type or "").strip().lower()
@@ -241,7 +246,7 @@ class StarPilotVCruise:
         self.standstill_force_stop_clear_since = 0.0
       elif self.standstill_force_stop_clear_since == 0.0:
         self.standstill_force_stop_clear_since = now
-      elif (now - self.standstill_force_stop_clear_since) >= STANDSTILL_FORCE_STOP_CLEAR_TIME:
+      elif self._elapsed_seconds(now, self.standstill_force_stop_clear_since) >= STANDSTILL_FORCE_STOP_CLEAR_TIME:
         self.standstill_force_stop_hold = False
         self.standstill_force_stop_clear_since = 0.0