tǔne

2026-06-28 18:12:05 +08:00 · 2026-05-01 18:57:37 -05:00
parent 708af332de
commit 762146babc
7 changed files with 188 additions and 42 deletions
@@ -102,12 +102,15 @@ PACIFICA_HYBRID_AOL_CARS = frozenset({
 })


+def pacifica_hybrid_aol_requires_set_press(car_fingerprint, pcm_cruise: bool) -> bool:
+  return car_fingerprint in PACIFICA_HYBRID_AOL_CARS and pcm_cruise
+
+
 def pacifica_hybrid_aol_stock_acc_mode(car_fingerprint, pcm_cruise: bool,
                                       controls_enabled: bool, always_on_lateral_enabled: bool) -> bool:
  # Keep this narrow until we have logs proving other Chrysler platforms need the same exemption.
  return (
-    car_fingerprint in PACIFICA_HYBRID_AOL_CARS and
-    pcm_cruise and
+    pacifica_hybrid_aol_requires_set_press(car_fingerprint, pcm_cruise) and
    always_on_lateral_enabled and
    not controls_enabled
  )
@@ -28,12 +28,12 @@ IONIQ_6_RESPONSE_MULTIPLIER = 1.2
 IONIQ_6_CANFD_SCC_ACCEL_STEP = (6.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER
 IONIQ_6_CANFD_SCC_DECEL_STEP = (15.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER
 GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5]
-GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.12, -0.12, -0.14, -0.18, -0.36, -0.72]
+GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.12, -0.12, -0.15, -0.22, -0.50, -0.95]
 GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5]
-GENESIS_G90_STOP_HOLD_RELAX_STEP_V = [0.10, 0.09, 0.08, 0.07, 0.05]
+GENESIS_G90_STOP_HOLD_RELAX_STEP_V = [0.08, 0.08, 0.06, 0.04, 0.025]
 GENESIS_G90_RELEASE_SPEED_BP = [0.0, 0.3, 0.6]
-GENESIS_G90_RELEASE_ACCEL_STEP_V = [0.24, 0.20, 0.14]
-GENESIS_G90_RELEASE_DECEL_STEP_V = [0.30, 0.24, 0.18]
+GENESIS_G90_RELEASE_ACCEL_STEP_V = [0.05, 0.07, 0.11]
+GENESIS_G90_RELEASE_DECEL_STEP_V = [0.16, 0.18, 0.18]
 GENESIS_G90_RELEASE_MAX_SPEED = 0.8
 IONIQ_6_LONG_MIN_JERK = 0.5 * IONIQ_6_RESPONSE_MULTIPLIER
 IONIQ_6_LONG_JERK_LIMIT = 4.8 * IONIQ_6_RESPONSE_MULTIPLIER
@@ -318,11 +318,11 @@ class TestHyundaiFingerprint:

    state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.5,
                                                   long_control_state=LongCtrlState.stopping, long_active=True)
-    assert state.actual_accel == pytest.approx(-1.95)
+    assert state.actual_accel == pytest.approx(-1.975)

    state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.3,
                                                   long_control_state=LongCtrlState.stopping, long_active=True)
-    assert state.actual_accel == pytest.approx(-1.88)
+    assert state.actual_accel == pytest.approx(-1.935)

  def test_genesis_g90_longitudinal_tuning_ramps_out_of_stop_hold(self):
    state = GenesisG90LongitudinalTuningState(actual_accel=-0.12, long_control_state_last=LongCtrlState.stopping)
@@ -330,11 +330,11 @@ class TestHyundaiFingerprint:
    state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=0.5, v_ego=0.02,
                                                   long_control_state=LongCtrlState.pid, long_active=True)
    assert state.release_active
-    assert state.actual_accel == pytest.approx(0.12)
+    assert state.actual_accel == pytest.approx(-0.06866666666666665)

    state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=0.5, v_ego=0.2,
                                                   long_control_state=LongCtrlState.pid, long_active=True)
-    assert state.actual_accel > 0.12
+    assert state.actual_accel == pytest.approx(-0.005333333333333315)

  def test_canfd_acc_control_uses_direct_accel(self):
    CP = CarParams.new_message()
@@ -89,6 +89,10 @@ VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.9
 VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.8
 VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.1
 VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25
+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

 # Uncertainty-based filter disable thresholds
 UNCERT_SLOPE_TRIG = 0.12  # per second
@@ -513,6 +517,28 @@ class LongitudinalPlanner:
    min_stop_brake = VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE + VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN * float(v_ego)
    return max(accel_min, -min_stop_brake), True

+  def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow):
+    if lead is None or not lead.status or v_ego < LEAD_CATCHUP_ACCEL_MIN_EGO:
+      return None
+
+    lead_delta = float(lead.vLead) - float(v_ego)
+    if lead_delta < LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA:
+      return None
+
+    desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
+    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
+    if gap_error > gap_buffer:
+      return None
+
+    # If the lead is already pace-matched or pulling away, keep any catch-up
+    # accel very small while we're near the follow target so we don't surge into
+    # the lead and immediately ask for brake again.
+    edge_cap = float(np.interp(lead_delta, [-0.5, 0.0, 1.0], [0.16, 0.08, 0.02]))
+    near_cap = min(edge_cap, 0.03)
+    gap_factor = float(np.clip(max(gap_error, 0.0) / max(gap_buffer, 0.1), 0.0, 1.0))
+    return float(np.interp(gap_factor, [0.0, 1.0], [near_cap, edge_cap]))
+
  @staticmethod
  def raw_close_lead_needs_control(lead, v_ego):
    if lead is None or not lead.status:
@@ -845,6 +871,12 @@ class LongitudinalPlanner:
      if moving_leads:
        output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL)

+    if lead_one_active:
+      lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, v_ego, effective_t_follow)
+      if lead_catchup_accel_cap is not None:
+        self.a_desired = min(self.a_desired, lead_catchup_accel_cap)
+        output_a_target = min(output_a_target, lead_catchup_accel_cap)
+
    if lead_control_active and np.isfinite(v_cruise) and any(lead.status for lead in (self.lead_one, self.lead_two)):
      # Keep follow/catchup behavior from pulling past the cruise target. Using the
      # same action horizon as the planner preserves normal accel farther below set speed.
@@ -422,6 +422,40 @@ def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_ver
  assert planner_with_lead.output_a_target < -0.2


+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
+def test_acc_mode_tracked_pace_matched_lead_caps_positive_catchup(model_version):
+  v_ego = 28.7
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego)
+  planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego)
+  sm_no_lead = make_sm(
+    v_ego,
+    desired_accel=0.5,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=False,
+  )
+  sm_with_lead = make_sm(
+    v_ego,
+    desired_accel=0.5,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=22.0, v_lead=29.4, a_lead=0.0, radar=False, model_prob=0.995),
+  )
+  sm_no_lead["starpilotPlan"].vCruise = v_ego + 4.0
+  sm_with_lead["starpilotPlan"].vCruise = v_ego + 4.0
+
+  for _ in range(8):
+    planner_no_lead.update(sm_no_lead, make_toggles(model_version))
+    planner_with_lead.update(sm_with_lead, make_toggles(model_version))
+
+  assert planner_with_lead.mode == "acc"
+  assert planner_with_lead.output_a_target <= planner_no_lead.output_a_target - 0.15
+  assert planner_with_lead.output_a_target < 0.08
+
+
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
 def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(model_version):
  v_ego = 3.8
@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+from opendbc.car.chrysler.values import pacifica_hybrid_aol_requires_set_press
 from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from openpilot.common.params import Params
 from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType
@@ -22,6 +23,7 @@ class StarPilotCard:
    self.accel_pressed = False
    self.always_on_lateral_allowed = False
    self.prev_active = False
+    self.prev_cruise_enabled = False
    self.decel_pressed = False
    self.distancePressed_previously = False
    self.force_coast = False
@@ -90,7 +92,15 @@ class StarPilotCard:
          elif starpilot_toggles.speed_limit_controller:
            self.params_memory.put_bool("SLCAdoptSpeedLimit", True)
    elif starpilot_toggles.always_on_lateral_main:
-      self.always_on_lateral_allowed = carState.cruiseState.available
+      if pacifica_hybrid_aol_requires_set_press(self.CP.carFingerprint, self.CP.pcmCruise):
+        # Chrysler Pacifica Hybrid stock ACC can fall back to plain cruise if AOL
+        # starts steering before the driver presses SET.
+        if not carState.cruiseState.available:
+          self.always_on_lateral_allowed = False
+        elif carState.cruiseState.enabled and not self.prev_cruise_enabled:
+          self.always_on_lateral_allowed = True
+      else:
+        self.always_on_lateral_allowed = carState.cruiseState.available

    # On rising edge of engagement (SET press enabling lat+long), auto-enable AOL
    # so that lateral persists when braking disengages longitudinal
@@ -98,6 +108,7 @@ class StarPilotCard:
      self.always_on_lateral_allowed = True

    self.prev_active = sm["selfdriveState"].active
+    self.prev_cruise_enabled = carState.cruiseState.enabled

    self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set
    self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS
@@ -1,5 +1,7 @@
 from types import SimpleNamespace

+from opendbc.car.chrysler.values import CAR as CHRYSLER_CAR
+
 from openpilot.starpilot.controls import starpilot_card as spc


@@ -29,6 +31,48 @@ class FakeSM(dict):
    self.updated = updated or {}


+def make_sm():
+  return FakeSM({
+    "carControl": SimpleNamespace(longActive=False),
+    "selfdriveState": SimpleNamespace(active=False, alertType=[], experimentalMode=False),
+    "starpilotSelfdriveState": SimpleNamespace(alertType=[]),
+    "starpilotPlan": SimpleNamespace(lateralCheck=True),
+    "liveCalibration": SimpleNamespace(calPerc=100),
+  }, updated={"starpilotPlan": False})
+
+
+def make_toggles(**overrides):
+  defaults = {
+    "always_on_lateral_lkas": False,
+    "always_on_lateral_main": False,
+    "always_on_lateral_pause_speed": 0.0,
+    "bookmark_via_lkas": False,
+    "conditional_experimental_mode": False,
+    "experimental_mode_via_lkas": False,
+    "force_coast_via_lkas": False,
+    "lkas_allowed_for_aol": False,
+    "pause_lateral_via_lkas": False,
+    "pause_longitudinal_via_lkas": False,
+    "speed_limit_controller": False,
+    "switchback_mode_via_lkas": False,
+    "traffic_mode_via_lkas": False,
+  }
+  defaults.update(overrides)
+  return SimpleNamespace(**defaults)
+
+
+def make_car_state(available=False, enabled=False, button_events=None):
+  return SimpleNamespace(
+    buttonEvents=button_events or [],
+    cruiseState=SimpleNamespace(available=available, enabled=enabled),
+    gearShifter=spc.GearShifter.drive,
+    brakePressed=False,
+    gasPressed=False,
+    standstill=False,
+    vEgo=15.0,
+  )
+
+
 def test_honda_lkas_button_can_toggle_always_on_lateral(monkeypatch, tmp_path):
  monkeypatch.setattr(spc, "Params", FakeParams)
  monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
@@ -36,39 +80,61 @@ def test_honda_lkas_button_can_toggle_always_on_lateral(monkeypatch, tmp_path):

  card = spc.StarPilotCard(SimpleNamespace(brand="honda"), SimpleNamespace(alternativeExperience=0))

-  car_state = SimpleNamespace(
-    buttonEvents=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)],
-    cruiseState=SimpleNamespace(available=False),
-    gearShifter=spc.GearShifter.drive,
-    brakePressed=False,
-    gasPressed=False,
-    standstill=False,
-    vEgo=15.0,
-  )
+  car_state = make_car_state(button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)])
  starpilot_car_state = SimpleNamespace(distancePressed=False)
-  sm = FakeSM({
-    "carControl": SimpleNamespace(longActive=False),
-    "selfdriveState": SimpleNamespace(active=False, alertType=[], experimentalMode=False),
-    "starpilotSelfdriveState": SimpleNamespace(alertType=[]),
-    "starpilotPlan": SimpleNamespace(lateralCheck=True),
-    "liveCalibration": SimpleNamespace(calPerc=100),
-  }, updated={"starpilotPlan": False})
-  toggles = SimpleNamespace(
-    always_on_lateral_lkas=True,
-    always_on_lateral_main=False,
-    always_on_lateral_pause_speed=0.0,
-    bookmark_via_lkas=False,
-    conditional_experimental_mode=False,
-    experimental_mode_via_lkas=False,
-    force_coast_via_lkas=False,
-    lkas_allowed_for_aol=True,
-    pause_lateral_via_lkas=False,
-    pause_longitudinal_via_lkas=False,
-    speed_limit_controller=False,
-    switchback_mode_via_lkas=False,
-    traffic_mode_via_lkas=False,
-  )
+  sm = make_sm()
+  toggles = make_toggles(always_on_lateral_lkas=True, lkas_allowed_for_aol=True)

  ret = card.update(car_state, starpilot_car_state, sm, toggles)

  assert ret.alwaysOnLateralAllowed is True
+
+
+def test_main_aol_still_follows_cruise_main_for_other_platforms(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(SimpleNamespace(brand="toyota", carFingerprint="TOYOTA_TEST", pcmCruise=True),
+                           SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL))
+
+  ret = card.update(make_car_state(available=True), SimpleNamespace(distancePressed=False), make_sm(),
+                    make_toggles(always_on_lateral_main=True))
+
+  assert ret.alwaysOnLateralAllowed is True
+  assert ret.alwaysOnLateralEnabled is True
+
+
+def test_pacifica_hybrid_main_aol_waits_for_set_press(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(
+    SimpleNamespace(brand="chrysler", carFingerprint=CHRYSLER_CAR.CHRYSLER_PACIFICA_2019_HYBRID, pcmCruise=True),
+    SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
+  )
+
+  sm = make_sm()
+  toggles = make_toggles(always_on_lateral_main=True)
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+  car_state = make_car_state(available=True, enabled=False)
+
+  ret = card.update(car_state, starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralAllowed is False
+  assert ret.alwaysOnLateralEnabled is False
+
+  car_state.cruiseState.enabled = True
+  ret = card.update(car_state, starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralAllowed is True
+  assert ret.alwaysOnLateralEnabled is True
+
+  car_state.cruiseState.enabled = False
+  ret = card.update(car_state, starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralAllowed is True
+  assert ret.alwaysOnLateralEnabled is True
+
+  car_state.cruiseState.available = False
+  ret = card.update(car_state, starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralAllowed is False
+  assert ret.alwaysOnLateralEnabled is False