From 7305822e67d942c08a8bcb9444f8cf5ce2a9b653 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 17 Jun 2026 11:16:00 -0500 Subject: [PATCH] leedle leedle leedle lee --- opendbc_repo/opendbc/car/gm/carcontroller.py | 6 +++- .../car/gm/tests/test_carcontroller.py | 19 +++++++++++++ selfdrive/controls/lib/desire_helper.py | 25 ++++++++++++++--- .../controls/tests/test_navigation_desires.py | 28 +++++++++++++++++++ starpilot/navigation/navigationd.py | 1 + 5 files changed, 74 insertions(+), 5 deletions(-) diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index 9289f1cca..7342a01dc 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -149,12 +149,13 @@ def estimate_auto_hold_brake(driver_brake: float, op_brake: float) -> int: def should_activate_auto_hold(hold_ready: bool, auto_hold_armed: bool, auto_hold_engaged: bool, - brake_pressed: bool, standstill: bool, long_active: bool, + brake_pressed: bool, gas_pressed: bool, standstill: bool, long_active: bool, regen_braking: bool, v_ego: float) -> bool: stopped = standstill or v_ego < 0.02 return ( hold_ready and (auto_hold_armed or auto_hold_engaged or brake_pressed) and + not gas_pressed and stopped and not long_active and not regen_braking @@ -398,6 +399,8 @@ class CarController(CarControllerBase): ) if not hold_ready or CS.out.gasPressed: CS.auto_hold_armed = False + if CS.out.gasPressed: + CS.auto_hold_engaged = False elif CS.regen_release_timer > 0.0: CS.auto_hold_armed = False elif not CS.auto_hold_armed and (CS.out.vEgo > 0.03 or ((CS.out.standstill or CS.out.vEgo < 0.02) and CS.out.brakePressed)): @@ -497,6 +500,7 @@ class CarController(CarControllerBase): CS.auto_hold_armed, CS.auto_hold_engaged, CS.out.brakePressed, + CS.out.gasPressed, CS.out.standstill, CC.longActive, CS.out.regenBraking, diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index 88c3accc5..383617933 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py @@ -236,6 +236,7 @@ def test_auto_hold_activation_allows_direct_entry_from_stopped_brake_press(): False, False, True, + False, True, False, False, @@ -249,6 +250,7 @@ def test_auto_hold_activation_stays_latched_after_brake_release(): False, True, False, + False, True, False, False, @@ -262,6 +264,7 @@ def test_auto_hold_activation_blocks_when_long_is_active_or_motion_is_above_thre True, False, True, + False, True, True, False, @@ -274,6 +277,7 @@ def test_auto_hold_activation_blocks_when_long_is_active_or_motion_is_above_thre False, False, False, + False, 0.03, ) @@ -284,6 +288,7 @@ def test_auto_hold_activation_allows_standstill_even_if_speed_filter_is_slightly True, False, False, + False, True, False, False, @@ -291,6 +296,20 @@ def test_auto_hold_activation_allows_standstill_even_if_speed_filter_is_slightly ) +def test_auto_hold_activation_releases_immediately_on_gas_press(): + assert not should_activate_auto_hold( + True, + True, + True, + False, + True, + True, + False, + False, + 0.0, + ) + + def test_friction_brake_mode_keeps_near_stop_disabled_for_regular_long_braking(): CP = SimpleNamespace(carFingerprint=CAR.CHEVROLET_VOLT_ASCM) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index d1bc71548..b5610ee71 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -17,6 +17,7 @@ NAV_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0] NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS = [0.0, 15.0, 30.0] NAV_KEEP_DISTANCE_BREAKPOINTS = [25.0, 90.0, 160.0] NAV_KEEP_AMBIGUOUS_SPLIT_DISTANCE_SCALE = 0.6 +NAV_KEEP_SMALL_SPLIT_MAX_OTHER_LANES = 2 DESIRES = { LaneChangeDirection.none: { @@ -127,14 +128,26 @@ class DesireHelper: return nudgeless @staticmethod - def _nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type="", same_side_lane_count=0): + def _nav_should_delay_ambiguous_split(maneuver_type="", same_side_lane_count=0, lane_count=0): + if maneuver_type not in ("off ramp", "fork") or int(same_side_lane_count or 0) <= 1: + return False + + total_lanes = int(lane_count or 0) + if total_lanes <= 0: + return True + + other_lanes = max(total_lanes - int(same_side_lane_count or 0), 0) + return other_lanes <= NAV_KEEP_SMALL_SPLIT_MAX_OTHER_LANES + + @staticmethod + def _nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type="", same_side_lane_count=0, lane_count=0): try: distance = float(maneuver_distance) except (TypeError, ValueError): return False threshold = float(np.interp(carstate.vEgo, NAV_KEEP_DISTANCE_SPEED_BREAKPOINTS, NAV_KEEP_DISTANCE_BREAKPOINTS)) - if maneuver_type in ("off ramp", "fork") and int(same_side_lane_count or 0) > 1: + if DesireHelper._nav_should_delay_ambiguous_split(maneuver_type, same_side_lane_count, lane_count): threshold *= NAV_KEEP_AMBIGUOUS_SPLIT_DISTANCE_SCALE return distance <= threshold @@ -148,8 +161,11 @@ class DesireHelper: if active_lane_direction not in ("slightLeft", "left", "sharpLeft", "slightRight", "right", "sharpRight"): return False + same_side_lane_count = int(nav_instruction_state.get("sameSideLaneCount", 0) or 0) + lane_count = int(nav_instruction_state.get("laneCount", 0) or 0) + return ( - int(nav_instruction_state.get("sameSideLaneCount", 0) or 0) > 1 and + DesireHelper._nav_should_delay_ambiguous_split(maneuver_type, same_side_lane_count, lane_count) and bool(nav_instruction_state.get("activeLaneAtRoadEdge", False)) and bool(nav_instruction_state.get("hasSharedSameSideLane", False)) ) @@ -160,9 +176,10 @@ class DesireHelper: maneuver_type = str(nav_instruction_state.get("maneuverType", "")) active_lane_direction = str(nav_instruction_state.get("activeLaneDirection", "")) same_side_lane_count = int(nav_instruction_state.get("sameSideLaneCount", 0) or 0) + lane_count = int(nav_instruction_state.get("laneCount", 0) or 0) if maneuver_type in ("off ramp", "fork") and modifier in ("slightLeft", "left", "sharpLeft", "slightRight", "right", "sharpRight"): - if not DesireHelper._nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type, same_side_lane_count): + if not DesireHelper._nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type, same_side_lane_count, lane_count): return "" if DesireHelper._nav_should_suppress_edge_lane_keep(nav_instruction_state): diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index b9cd35420..a856f043e 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -175,6 +175,7 @@ def test_nav_desires_edge_exit_lane_with_shared_transition_lane_does_not_keep_ri "maneuverType": "off ramp", "maneuverModifier": "right", "activeLaneDirection": "slightRight", + "laneCount": 3, "sameSideLaneCount": 2, "activeLaneAtRoadEdge": True, "hasSharedSameSideLane": True, @@ -192,6 +193,33 @@ def test_nav_desires_edge_exit_lane_with_shared_transition_lane_does_not_keep_ri assert helper.desire == log.Desire.none +def test_nav_desires_wide_highway_edge_exit_lane_keeps_right(): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = { + "valid": True, + "maneuverType": "off ramp", + "maneuverModifier": "right", + "activeLaneDirection": "slightRight", + "laneCount": 7, + "sameSideLaneCount": 2, + "activeLaneAtRoadEdge": True, + "hasSharedSameSideLane": True, + "maneuverDistance": 105.0, + } + + helper.update( + make_car_state(vEgo=19.0), + True, + 0.0, + make_plan(laneWidthRight=4.2), + make_toggles(nudgeless=True), + ) + + assert helper.desire == log.Desire.keepRight + + def test_nav_desires_shared_transition_lane_keeps_when_active_lane_is_not_outermost(): helper = DesireHelper() helper.nav_desires_allowed = True diff --git a/starpilot/navigation/navigationd.py b/starpilot/navigation/navigationd.py index a8f0a710b..5ed6db3fa 100644 --- a/starpilot/navigation/navigationd.py +++ b/starpilot/navigation/navigationd.py @@ -292,6 +292,7 @@ class Navigationd: "valid": True, "maneuverModifier": str(payload.get("maneuverModifier") or ""), "maneuverType": str(payload.get("maneuverType") or ""), + "laneCount": len(lanes), "activeLaneDirection": active_lane_direction, "activeLaneIndex": active_lane_index, "activeLaneAtRoadEdge": active_lane_at_road_edge,