diff --git a/opendbc_repo/opendbc/car/chrysler/values.py b/opendbc_repo/opendbc/car/chrysler/values.py index cd05100ea..ef04f841c 100644 --- a/opendbc_repo/opendbc/car/chrysler/values.py +++ b/opendbc_repo/opendbc/car/chrysler/values.py @@ -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 ) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index d82f2f152..2229a8cb1 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -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 diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 6eb1278ea..86b5bd53c 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -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() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index fe44606af..4718df678 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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. diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 2b94f5f79..0584fae26 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -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 diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index f24668909..82ec99982 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -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 diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 124040adb..9bcb6d348 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -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