nah

openpilot/selfdrive/controls/lib/desire_helper.py

@@ -64,16 +64,19 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
-                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
+                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
-        self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
+        edge_detected = ((left_edge_detected and self.lane_change_direction == LaneChangeDirection.left) or
+                         (right_edge_detected and self.lane_change_direction == LaneChangeDirection.right))
+
+        self.alc.update_lane_change(blindspot_detected, edge_detected, carstate.brakePressed, torque_applied)
 
         if not one_blinker or below_lane_change_speed:
           self.lane_change_state = LaneChangeState.off
           self.lane_change_direction = LaneChangeDirection.none
           self.lane_change_timer = 0.0
-        elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected:
+        elif (torque_applied or self.alc.auto_lane_change_allowed) and not self.alc.lane_change_blocked:
           self.lane_change_state = LaneChangeState.laneChangeStarting
           self.lane_change_timer = 0.0
 

openpilot/sunnypilot/selfdrive/controls/lib/auto_lane_change.py

@@ -10,6 +10,8 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 
 
+
+
 class AutoLaneChangeMode:
   OFF = -1
   NUDGE = 0  # default
@@ -31,6 +33,7 @@ AUTO_LANE_CHANGE_TIMER = {
 }
 
 ONE_SECOND_DELAY = -1
+EDGE_DELAY = -2
 
 
 class AutoLaneChangeController:
@@ -48,6 +51,7 @@ class AutoLaneChangeController:
     self.prev_brake_pressed = False
     self.auto_lane_change_allowed = False
     self.prev_lane_change = False
+    self.lane_change_blocked = False
 
     self.read_params()
 
@@ -68,7 +72,7 @@ class AutoLaneChangeController:
       self.read_params()
     self.param_read_counter += 1
 
-  def update_lane_change_timers(self, blindspot_detected: bool) -> None:
+  def update_lane_change_timers(self, blindspot_detected: bool, edge_detected: bool) -> None:
     self.lane_change_delay = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer,
                                                         AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGE])
 
@@ -80,6 +84,12 @@ class AutoLaneChangeController:
       else:
         self.lane_change_wait_timer = self.lane_change_delay + ONE_SECOND_DELAY
 
+    if edge_detected and self.lane_change_delay > 0:
+      if self.lane_change_delay == AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]:
+        self.lane_change_wait_timer = min(self.lane_change_wait_timer, EDGE_DELAY)
+      else:
+        self.lane_change_wait_timer = min(self.lane_change_wait_timer, self.lane_change_delay + EDGE_DELAY)
+
   def update_allowed(self) -> bool:
     # Auto lane change allowed if:
     # 1. A valid delay is set (non-zero)
@@ -97,14 +107,23 @@ class AutoLaneChangeController:
 
     return bool(self.lane_change_wait_timer > self.lane_change_delay)
 
-  def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
+  def update_lane_change(self, blindspot_detected: bool, edge_detected: bool, brake_pressed: bool,
+                         torque_applied: bool = False) -> None:
     if brake_pressed and not self.prev_brake_pressed:
       self.prev_brake_pressed = brake_pressed
 
-    self.update_lane_change_timers(blindspot_detected)
+    self.update_lane_change_timers(blindspot_detected, edge_detected)
 
     self.auto_lane_change_allowed = self.update_allowed()
 
+    # BSM always blocks. Edge blocks auto mode but nudge can override.
+    if blindspot_detected:
+      self.lane_change_blocked = True
+    elif edge_detected:
+      self.lane_change_blocked = not torque_applied
+    else:
+      self.lane_change_blocked = False
+
   def update_state(self):
     if self.DH.lane_change_state == log.LaneChangeState.laneChangeStarting:
       self.prev_lane_change = True

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py

@@ -8,7 +8,7 @@ from openpilot.common.parameterized import parameterized
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode, \
-  AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY
+  AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY, EDGE_DELAY
 from openpilot.common.test import OpenpilotTestCase
 
 AUTO_LANE_CHANGE_TIMER_COMBOS = [
@@ -61,7 +61,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     # Update controller
     num_updates = int(5.0 / DT_MDL)
     for _ in range(num_updates):  # Run for 5 seconds
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Mode should not allow lane change immediately
     assert not self.alc.auto_lane_change_allowed
@@ -74,11 +74,11 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
 
     # Update controller once to read params
-    self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Update multiple times to exceed the timer threshold
     for _ in range(1):  # Should exceed 0.1s with multiple DT_MDL updates
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Now lane change should be allowed
     assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
@@ -91,7 +91,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     self.alc.lane_change_set_timer = timer_state
 
     # Update controller once
-    self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # The timer should still be below the threshold after one update
     assert not self.alc.auto_lane_change_allowed
@@ -99,7 +99,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     # Update enough times to exceed the threshold (seconds / DT_MDL)
     num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Now lane change should be allowed
     assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
@@ -116,7 +116,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
 
     # Update with brake pressed for 1 second
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=True)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=True)
 
     # Even though it is an auto lane change mode, lane change should be disallowed due to brake pressed prior initiating lane change
     assert not self.alc.auto_lane_change_allowed
@@ -126,7 +126,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
 
     # Even releasing brake shouldn't allow auto lane change
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     assert not self.alc.auto_lane_change_allowed
 
@@ -139,13 +139,13 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     self.alc.lane_change_set_timer = timer_state
 
     # Update with blindspot detected - this should prevent auto lane change
-    self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False)
     assert not self.alc.auto_lane_change_allowed
 
     # Keep updating with blindspot detected - should still prevent auto lane change
     num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False)
     assert not self.alc.auto_lane_change_allowed
 
   @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
@@ -157,13 +157,13 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     self.alc.lane_change_set_timer = timer_state
 
     # First update with blindspot detected to set the negative timer
-    self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False)
     assert not self.alc.auto_lane_change_allowed
 
     # Now update with blindspot cleared - should start incrementing timer from negative value
     num_updates = int((timer_delay + abs(ONE_SECOND_DELAY)) / DT_MDL) + 1
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # After sufficient updates with no blindspot, auto lane change should be allowed
     assert self.alc.auto_lane_change_allowed
@@ -177,7 +177,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
 
     # Update enough times to exceed the threshold (seconds / DT_MDL)
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Now lane change should be allowed
     assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
@@ -193,7 +193,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
 
     # Update enough times to exceed the threshold (seconds / DT_MDL)
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     assert not self.alc.auto_lane_change_allowed
 
@@ -206,7 +206,86 @@ class TestAutoLaneChangeController(OpenpilotTestCase):
     # Simulate updates for a long period of time (e.g., 10 seconds)
     num_updates = int(10.0 / DT_MDL)
     for _ in range(num_updates):
-      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
 
     # Lane change should never be allowed
     assert not self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_edge_detected_delays_auto_lane_change(self, timer_state, timer_delay):
+    """Test that road edge detection adds delay to auto lane change."""
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = timer_state
+
+    # Update with edge detected - should prevent auto lane change
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Keep updating with edge detected - timer keeps resetting, should stay disallowed
+    num_updates = int(timer_delay / DT_MDL) + 1
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_edge_detected_then_cleared_allows_after_delay(self, timer_state, timer_delay):
+    """Test that auto lane change is allowed after edge clears plus extra delay."""
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = timer_state
+
+    # First update with edge detected to set the negative timer
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Now update with edge cleared - should start incrementing timer from negative value
+    num_updates = int((timer_delay + abs(EDGE_DELAY)) / DT_MDL) + 1
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
+
+    # After sufficient updates with no edge, auto lane change should be allowed
+    assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_edge_and_bsm_combined_uses_worst_delay(self, timer_state, timer_delay):
+    """Test that when both BSM and edge are detected, the worst (longest) delay wins."""
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = True
+    self.alc.lane_change_set_timer = timer_state
+
+    # Update with both blindspot and edge detected
+    self.alc.update_lane_change(blindspot_detected=True, edge_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Clear both - need to wait out the worst delay
+    worst_delay = max(abs(ONE_SECOND_DELAY), abs(EDGE_DELAY))
+    num_updates = int((timer_delay + worst_delay) / DT_MDL) + 1
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False)
+
+    assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_nudge_overrides_edge_in_lane_change_blocked(self, timer_state, timer_delay):
+    """Test that lane_change_blocked is False when nudge applied with edge-only."""
+    self._reset_states()
+    self.alc.lane_change_set_timer = timer_state
+
+    # Edge detected, no nudge - should be blocked
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False, torque_applied=False)
+    assert self.alc.lane_change_blocked
+
+    # Edge detected, nudge applied - should NOT be blocked
+    self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False, torque_applied=True)
+    assert not self.alc.lane_change_blocked
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_nudge_does_not_override_bsm_in_lane_change_blocked(self, timer_state, timer_delay):
+    """Test that lane_change_blocked stays True when BSM detected even with nudge."""
+    self._reset_states()
+    self.alc.lane_change_set_timer = timer_state
+
+    # BSM detected with nudge - should still be blocked
+    self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False, torque_applied=True)
+    assert self.alc.lane_change_blocked

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -113,12 +113,56 @@ class TestDesireHelperIntegration(OpenpilotTestCase):
                 left_edge_detected=False, right_edge_detected=False)
     assert dh.desire == expected_desire
 
-  def test_edge_blocks_lane_change(self, set_lane_turn_params):
+  def test_nudge_overrides_edge_detection(self, set_lane_turn_params):
+    """Manual steering nudge should override road edge detection and allow lane change."""
     dh = DesireHelper()
     dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
     carstate = DummyCarState(vEgo=15, leftBlinker=True, steeringPressed=True, steeringTorque=1)
     for _ in range(10):
       dh.update(carstate, True, 1.0, left_edge_detected=True, right_edge_detected=False)
-    assert dh.lane_change_state == LaneChangeState.preLaneChange
+    assert dh.lane_change_state == LaneChangeState.laneChangeStarting
     assert dh.lane_change_direction == LaneChangeDirection.left
+    assert dh.desire == log.Desire.laneChangeLeft
+
+  def test_nudge_does_not_override_bsm(self, set_lane_turn_params):
+    """Manual steering nudge must NOT override BSM blindspot detection."""
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+    carstate = DummyCarState(vEgo=15, leftBlinker=True, leftBlindspot=True, steeringPressed=True, steeringTorque=1)
+    for _ in range(10):
+      dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=False)
+    assert dh.lane_change_state == LaneChangeState.preLaneChange
     assert dh.desire == log.Desire.none
+
+  def test_edge_blocks_auto_lane_change(self, set_lane_turn_params):
+    """Road edge detection should still block auto (nudgeless) lane changes."""
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+    dh.alc.param_read_counter = 1
+    carstate = DummyCarState(vEgo=15, leftBlinker=True)
+    for _ in range(10):
+      dh.update(carstate, True, 1.0, left_edge_detected=True, right_edge_detected=False)
+    assert dh.lane_change_state == LaneChangeState.preLaneChange
+    assert dh.desire == log.Desire.none
+
+  def test_auto_lane_change_allowed_without_edge(self, set_lane_turn_params):
+    """Auto lane change should proceed when no edge and no BSM detected."""
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+    dh.alc.param_read_counter = 1
+    carstate = DummyCarState(vEgo=15, leftBlinker=True)
+    for _ in range(10):
+      dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=False)
+    assert dh.lane_change_state == LaneChangeState.laneChangeStarting
+    assert dh.desire == log.Desire.laneChangeLeft
+
+  def test_right_edge_does_not_block_left_lane_change(self, set_lane_turn_params):
+    """Edge on opposite side should not affect lane change direction."""
+    dh = DesireHelper()
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+    dh.alc.param_read_counter = 1
+    carstate = DummyCarState(vEgo=15, leftBlinker=True)
+    for _ in range(10):
+      dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=True)
+    assert dh.lane_change_state == LaneChangeState.laneChangeStarting
+    assert dh.desire == log.Desire.laneChangeLeft