From 16c3d5f48ab05fe713aed3f1ab89b97ff1c4f5c3 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 23 Jun 2026 21:45:36 -0500 Subject: [PATCH] Tom Bombadil --- selfdrive/controls/lib/lead_behavior.py | 39 +++++++++++++-- .../lib/longitudinal_mpc_lib/long_mpc.py | 48 ++++++++++++++++++- .../controls/tests/test_lead_behavior.py | 14 ++++++ .../tests/test_longitudinal_planner.py | 30 ++++++++++++ starpilot/controls/starpilot_card.py | 9 ++++ .../controls/tests/test_starpilot_card.py | 45 +++++++++++++++++ 6 files changed, 180 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index ed47945e3..b97b2a310 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -7,6 +7,16 @@ VISION_LEAD_TRACK_MIN_DISTANCE = 25.0 VISION_LEAD_TRACK_BASE_TIME_GAP = 1.75 VISION_LEAD_TRACK_CLOSING_GAIN = 0.20 VISION_LEAD_TRACK_CLOSING_CAP = 2.50 +TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.35 +TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.65 +TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN = 0.75 +TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN = 1.05 +TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START = 2.75 +TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END = 3.10 +TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET = 0.90 +TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET = 1.60 +TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.65 +TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR = 0.75 RADARLESS_MATCHED_FOLLOW_MIN_SPEED = 22.0 RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED = 2.0 RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY = 0.95 @@ -56,10 +66,11 @@ def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lea def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float, - v_cruise: float | None = None) -> float: + v_cruise: float | None = None, y_rel: float | None = None) -> float: gap_error = lead_distance - desired_gap actual_hw = lead_distance / max(v_ego, 1e-3) desired_hw = desired_gap / max(v_ego, 1e-3) + headway_margin = actual_hw - desired_hw if v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED: return 0.0 @@ -71,14 +82,34 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga # Encourage ACC to treat a tracked lead as the active constraint when we're # hanging far above the requested time gap, but don't override cruise for a # truly distant lead or one we're already closing on decisively. - if actual_hw <= max(desired_hw + 0.3, 1.72): + if headway_margin <= TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN: return 0.0 - if actual_hw >= max(desired_hw + 1.6, 3.0): + fade_start_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN, + TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START - desired_hw) + fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN, + TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw) + if headway_margin >= fade_end_margin: return 0.0 if closing_speed > max(2.5, 0.12 * v_ego): return 0.0 - return min(gap_error * 0.65, max(14.0, 0.75 * v_ego)) + entry_factor = min(1.0, max(0.0, (headway_margin - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN) / + max(TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN, 1e-3))) + exit_factor = 1.0 + if headway_margin > fade_start_margin: + exit_factor = min(1.0, max(0.0, (fade_end_margin - headway_margin) / max(fade_end_margin - fade_start_margin, 1e-3))) + + lateral_factor = 1.0 + if y_rel is not None: + lateral_offset = abs(float(y_rel)) + if lateral_offset >= TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET: + return 0.0 + if lateral_offset > TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET: + lateral_factor = min(1.0, max(0.0, (TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET - lateral_offset) / + max(TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET - TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET, 1e-3))) + + bias_cap = max(14.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego) + return min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * entry_factor * exit_factor * lateral_factor def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index b3816c165..051953633 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -99,6 +99,12 @@ IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_HEADWAY_ABOVE_TARGET = 0.40 IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_LEAD_BRAKE = 0.25 IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_PULLAWAY_SPEED = 1.5 IDENTICAL_RADAR_DUPLICATE_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 10.0 +IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_SPEED = 15.0 +IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET = 0.55 +IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET = -0.15 +IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_LEAD_BRAKE = 0.25 +IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_PULLAWAY_SPEED = 0.75 +IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX = 10.0 # Function to get parameter value based on current speed def get_speed_based_param(speed_mph, param_array): @@ -722,6 +728,37 @@ class LongitudinalMpc: return prev_source + def get_identical_radar_duplicate_cruise_bias(self, lead_one, lead_two, v_ego, t_follow): + if float(v_ego) < IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_SPEED: + return 0.0 + if not self.leads_share_identical_radar_track(lead_one, lead_two): + return 0.0 + + lead = lead_one if lead_one.status else lead_two + if lead is None or not lead.status: + return 0.0 + + actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) + headway_margin = actual_headway - float(t_follow) + if headway_margin < IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET: + return 0.0 + if headway_margin > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET: + return 0.0 + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_LEAD_BRAKE: + return 0.0 + + lead_delta = float(lead.vLead) - float(v_ego) + if lead_delta > IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_PULLAWAY_SPEED: + return 0.0 + + return float(np.interp( + headway_margin, + [IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_HEADWAY_BELOW_TARGET, 0.0, IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX_HEADWAY_ABOVE_TARGET], + [IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX, IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MAX * 0.85, 0.0], + )) + def set_accel_limits(self, min_a, max_a): # TODO this sets a max accel limit, but the minimum limit is only for cruise decel # needs refactor @@ -769,7 +806,16 @@ class LongitudinalMpc: if optional_far_lead_comfort and tracking_lead and lead_one.status: desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow) closing_speed = max(0.0, v_ego - lead_one.vLead) - cruise_obstacle += get_tracked_lead_catchup_bias(v_ego, lead_one.dRel, desired_gap, closing_speed, v_cruise=v_cruise) + cruise_obstacle += get_tracked_lead_catchup_bias( + v_ego, + lead_one.dRel, + desired_gap, + closing_speed, + v_cruise=v_cruise, + y_rel=float(getattr(lead_one, "yRel", 0.0)), + ) + if optional_far_lead_comfort: + cruise_obstacle += self.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow) if optional_far_lead_comfort: lead_0_bias, lead_1_bias = self.get_near_duplicate_lead_source_hysteresis(prev_source, lead_one, lead_two, v_ego) lead_0_obstacle = lead_0_obstacle + lead_0_bias diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index b4d9cfb97..defcd6fe0 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -26,6 +26,20 @@ def test_tracked_lead_catchup_bias_applies_to_two_second_highway_gap(): assert bias > 14.0 +def test_tracked_lead_catchup_bias_reduces_for_laterally_offset_lead(): + centered = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9, y_rel=0.2) + offset = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9, y_rel=1.95) + assert centered > 0.0 + assert offset == 0.0 + + +def test_tracked_lead_catchup_bias_fades_before_very_far_gap_cutoff(): + near_upper = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9) + smaller_gap = get_tracked_lead_catchup_bias(34.0, 96.0, 73.0, 1.9) + assert near_upper > 0.0 + assert near_upper < smaller_gap + + def test_tracked_lead_catchup_bias_stays_off_once_at_set_speed(): bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1, v_cruise=31.4) assert bias == 0.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 3f2fff3d3..84de22133 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -3039,6 +3039,36 @@ def test_identical_radar_duplicate_cruise_hold_skips_clear_pullaway(): assert sticky is None +def test_identical_radar_duplicate_cruise_bias_penalizes_near_target_follow(): + v_ego = 23.8 + t_follow = 1.15 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=35.8, v_lead=23.2, a_lead=0.02, radar=True, model_prob=1.0) + lead_two = make_lead(status=True, d_rel=35.8, v_lead=23.2, a_lead=0.02, radar=True, model_prob=1.0) + lead_one.radarTrackId = 2493 + lead_two.radarTrackId = 2493 + + bias = planner.mpc.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow) + + assert bias > 0.0 + + +def test_identical_radar_duplicate_cruise_bias_skips_far_pullaway_follow(): + v_ego = 23.8 + t_follow = 1.15 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=52.0, v_lead=25.5, a_lead=0.08, radar=True, model_prob=1.0) + lead_two = make_lead(status=True, d_rel=52.0, v_lead=25.5, a_lead=0.08, radar=True, model_prob=1.0) + lead_one.radarTrackId = 2493 + lead_two.radarTrackId = 2493 + + bias = planner.mpc.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow) + + assert bias == 0.0 + + def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index a5a1a5120..03951671b 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -37,6 +37,7 @@ class StarPilotCard: getattr(self.CP, "carFingerprint", None) in (HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, HYUNDAI_CAR.KIA_FORTE_2021_NON_SCC) and bool(hyundai_flags & HyundaiFlags.NON_SCC) ) + self.hyundai_lkas_aol_requires_engagement = getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID self.hyundai_aol_needs_engagement = self.CP.brand == "hyundai" and not (hyundai_flags & HyundaiFlags.CANFD) and not kia_forte_non_scc self.hyundai_aol_ready = False self.prev_active = False @@ -113,6 +114,12 @@ class StarPilotCard: def update(self, carState, starpilotCarState, sm, starpilot_toggles): self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled") button_event_types = [self._button_type_raw(be) for be in carState.buttonEvents] + hyundai_lkas_aol_can_toggle = ( + not self.hyundai_lkas_aol_requires_engagement or + self.hyundai_aol_ready or + sm["selfdriveState"].active or + carState.cruiseState.enabled + ) if self.hyundai_aol_needs_engagement: if carState.gearShifter in NON_DRIVING_GEARS: @@ -124,6 +131,8 @@ class StarPilotCard: if self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol: for be, be_type in zip(carState.buttonEvents, button_event_types, strict=False): if be_type == ButtonType.lkas and be.pressed and starpilot_toggles.always_on_lateral_lkas: + if not hyundai_lkas_aol_can_toggle: + continue if self.hyundai_aol_needs_engagement: self.hyundai_aol_ready = True self.always_on_lateral_allowed = not self.always_on_lateral_allowed diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 8bb7b4924..4ba715c52 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -146,6 +146,51 @@ def test_hyundai_lkas_button_can_start_aol_before_normal_engagement(monkeypatch, assert ret.pauseLateral is False +def test_sonata_hybrid_lkas_button_does_not_start_aol_before_engagement(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="hyundai", carFingerprint=spc.HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + + car_state = make_car_state(available=False, enabled=False, button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)]) + starpilot_car_state = SimpleNamespace(distancePressed=False) + sm = make_sm() + toggles = make_toggles(always_on_lateral=True, always_on_lateral_lkas=True) + + ret = card.update(car_state, starpilot_car_state, sm, toggles) + + assert ret.alwaysOnLateralAllowed is False + assert ret.alwaysOnLateralEnabled is False + + +def test_sonata_hybrid_lkas_button_can_toggle_aol_after_engagement(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="hyundai", carFingerprint=spc.HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + + starpilot_car_state = SimpleNamespace(distancePressed=False) + sm = make_sm() + toggles = make_toggles(always_on_lateral=True, always_on_lateral_lkas=True) + + engaged_state = make_car_state(available=True, enabled=True) + card.update(engaged_state, starpilot_car_state, sm, toggles) + + lkas_state = make_car_state(available=False, enabled=False, button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)]) + ret = card.update(lkas_state, starpilot_car_state, sm, toggles) + + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is True + + def test_hyundai_aol_does_not_auto_start_from_cruise_availability(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)