diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 93088182f9..eb2dd0fd48 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -31,12 +31,6 @@ DESIRES = { }, } -TURN_DESIRES = { - custom.TurnDirection.none: log.Desire.none, - custom.TurnDirection.turnLeft: log.Desire.turnLeft, - custom.TurnDirection.turnRight: log.Desire.turnRight, -} - class DesireHelper: def __init__(self): @@ -121,8 +115,10 @@ class DesireHelper: self.prev_one_blinker = one_blinker - if self.lane_turn_direction != custom.TurnDirection.none: - self.desire = TURN_DESIRES[self.lane_turn_direction] + lane_change_nudge_mode = self.alc.lane_change_set_timer == AutoLaneChangeMode.NUDGE + turn_desire = self.lane_turn_controller.get_desire(lane_change_nudge_mode) + if turn_desire != log.Desire.none: + self.desire = turn_desire else: self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] diff --git a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py index 00ce026abb..6fcc216dc2 100644 --- a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py +++ b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py @@ -4,13 +4,20 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ -from cereal import custom +from cereal import custom, log from openpilot.common.constants import CV from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS +TURN_DESIRES = { + custom.TurnDirection.none: log.Desire.none, + custom.TurnDirection.turnLeft: log.Desire.turnLeft, + custom.TurnDirection.turnRight: log.Desire.turnRight, +} + class LaneTurnController: def __init__(self, desire_helper): @@ -20,6 +27,7 @@ class LaneTurnController: self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS self.param_read_counter = 0 self.enabled = self.params.get_bool("LaneTurnDesire") + self.lane_turn_nudge_timer = 0.0 def read_params(self): self.enabled = self.params.get_bool("LaneTurnDesire") @@ -43,3 +51,24 @@ class LaneTurnController: if not self.enabled: return custom.TurnDirection.none return self.turn_direction + + def get_desire(self, nudge_mode: bool) -> log.Desire: + # Return the current Desire for lane-turns. + # disabled or no turn -> reset timer and return none + if not self.enabled or self.turn_direction == custom.TurnDirection.none: + self.lane_turn_nudge_timer = 0.0 + return log.Desire.none + + # Normal (non-nudge) behavior: persistent turn desire + if not nudge_mode: + return TURN_DESIRES[self.turn_direction] + + # Nudge mode: pulse a keep desire once per second + self.lane_turn_nudge_timer += DT_MDL + if self.lane_turn_nudge_timer > 1.0: + # reset and show the short pulse + self.lane_turn_nudge_timer = 0.0 + return TURN_DESIRES[self.turn_direction] + + # most of the second the desire is cleared + return log.Desire.none diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py index 5633ed6efc..24aa3e47cb 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py @@ -21,7 +21,8 @@ class TurnDirection: (False, True, 6, False, True, TurnDirection.none), (False, False, 5, False, False, TurnDirection.none), (True, True, 5, False, False, TurnDirection.none), -]) +], ids=["left blinker", "right blinker", "no blinkers", "left blindspot w/ left blinker", "right blindspot w/ left blinker", + "no blinkers", "both blinkers"]) def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected): dh = DesireHelper() controller = LaneTurnController(dh) @@ -63,7 +64,7 @@ def test_lane_turn_overrides_lane_change(): (8.93, TurnDirection.turnLeft), # just below threshold (8.96, TurnDirection.none), # above threshold (8.95, TurnDirection.none), # just above threshold -]) +], ids=["below threshold", "above threshold", "just above threshold"]) def test_lane_turn_desire_speed_boundary(v_ego, expected): dh = DesireHelper() controller = LaneTurnController(dh) @@ -91,6 +92,7 @@ def set_lane_turn_params(): params = Params() params.put("LaneTurnDesire", True) params.put("LaneTurnValue", 20.0) + params.put("AutoLaneChangeTimer", 1) @pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [ # Lane turn desire overrides lane change desire @@ -104,10 +106,15 @@ def set_lane_turn_params(): # No desire (inactive) (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none), (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none), # No blinkers? no desire! -]) +], ids=["turn left", "turn right", "lane change left", "lane change right", "inactive", "no blinkers"]) def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params): dh = DesireHelper() + + dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS + dh.update(carstate, lateral_active, lane_change_prob) + dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE - for _ in range(10): - dh.update(carstate, lateral_active, lane_change_prob) - assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers + for _ in range(20): # nudge mode requires 20 steps (.05 * 20 = 1 second) to simulate one second to enter true state + dh.update(carstate, lateral_active, lane_change_prob) + + assert dh.desire == expected_desire # This tests the integration in desire helpers