leedle leedle leedle lee

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,

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)
 

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):

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

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,