diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 634ad2e1f..ec21144e6 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -191,9 +191,12 @@ class DesireHelper: v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < starpilot_toggles.minimum_lane_change_speed + cruise_state = getattr(carstate, "cruiseState", None) + lane_changes_allowed = starpilot_toggles.lane_changes + lane_changes_allowed &= not getattr(starpilot_toggles, "lane_changes_require_cruise", False) or bool(getattr(cruise_state, "enabled", False)) lane_change_time_max = getattr(starpilot_toggles, 'lane_change_time_max', LANE_CHANGE_TIME_MAX) - if not lateral_active or self.lane_change_timer > lane_change_time_max or not starpilot_toggles.lane_changes: + if not lateral_active or self.lane_change_timer > lane_change_time_max or not lane_changes_allowed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index 1b1ce851f..77f351c36 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -15,6 +15,7 @@ def make_car_state(**overrides): "steeringPressed": False, "steeringTorque": 0.0, "standstill": False, + "cruiseState": SimpleNamespace(enabled=True), } defaults.update(overrides) return SimpleNamespace(**defaults) @@ -29,6 +30,7 @@ def make_toggles(**overrides): "nudgeless": True, "one_lane_change": False, "use_turn_desires": False, + "lane_changes_require_cruise": False, } defaults.update(overrides) return SimpleNamespace(**defaults) @@ -229,6 +231,52 @@ def test_nav_desires_do_not_override_lane_change_state_machine(): assert helper.desire == log.Desire.laneChangeLeft +def test_lane_changes_require_cruise_blocks_blinker_lane_change_without_cruise(): + helper = DesireHelper() + + helper.update( + make_car_state(leftBlinker=True, cruiseState=SimpleNamespace(enabled=False)), + True, + 0.0, + make_plan(), + make_toggles(lane_changes_require_cruise=True), + ) + + assert helper.lane_change_state == LaneChangeState.off + assert helper.lane_change_direction == LaneChangeDirection.none + assert helper.desire == log.Desire.none + + +def test_lane_changes_require_cruise_allows_blinker_lane_change_with_cruise(): + helper = DesireHelper() + + helper.update( + make_car_state(leftBlinker=True, cruiseState=SimpleNamespace(enabled=True)), + True, + 0.0, + make_plan(), + make_toggles(lane_changes_require_cruise=True), + ) + + assert helper.lane_change_state == LaneChangeState.preLaneChange + assert helper.lane_change_direction == LaneChangeDirection.left + + +def test_lane_changes_without_cruise_requirement_keep_existing_behavior(): + helper = DesireHelper() + + helper.update( + make_car_state(leftBlinker=True, cruiseState=SimpleNamespace(enabled=False)), + True, + 0.0, + make_plan(), + make_toggles(lane_changes_require_cruise=False), + ) + + assert helper.lane_change_state == LaneChangeState.preLaneChange + assert helper.lane_change_direction == LaneChangeDirection.left + + def test_nav_desires_disabled_leave_desire_unchanged(): helper = DesireHelper() helper.nav_desires_allowed = False diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 8baf7e9b6..70d281951 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -955,6 +955,7 @@ class StarPilotVariables: toggle.wheel_image = toggle.current_holiday_theme toggle.lane_changes = self.get_value("LaneChanges") + toggle.lane_changes_require_cruise = toggle.car_model == HYUNDAI_CAR.KIA_XCEED_PHEV toggle.lane_change_delay = self.get_value("LaneChangeTime", cast=float, condition=toggle.lane_changes) toggle.lane_detection_width = self.get_value("LaneDetectionWidth", cast=float, condition=toggle.lane_changes, conversion=distance_conversion) toggle.minimum_lane_change_speed = self.get_value("MinimumLaneChangeSpeed", cast=float, condition=toggle.lane_changes, conversion=speed_conversion)