diff --git a/common/libcommon.a b/common/libcommon.a index d86f6ee53..f3262fec1 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index feedd84da..334558f13 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -424,6 +424,7 @@ inline static std::unordered_map keys = { {"MapsSelected", {PERSISTENT, STRING, "", "", 0}}, {"MapSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}}, {"NavDesiresAllowed", {PERSISTENT, BOOL, "1", "0", 2}}, + {"NavLanePositioningAllowed", {PERSISTENT, BOOL, "0", "0", 2}}, {"NavLongitudinalAllowed", {PERSISTENT, BOOL, "1", "0", 2}}, {"ClearNavOnOffroad", {PERSISTENT, BOOL, "1", "1", 2, SETTINGS_SIMPLE}}, {"ClearNavOnOffroadTimeoutMinutes", {PERSISTENT, INT, "0", "0", 2, SETTINGS_SIMPLE}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 27b62573d..363d891a5 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 5972bda20..1a9585098 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -66,6 +66,7 @@ class DesireHelper: self.lane_change_wait_timer = 0.0 self.nav_desires_allowed = False + self.nav_lane_positioning_allowed = False self._nav_instruction_state_raw: object = None self._nav_instruction_state: dict[str, object] = {} @@ -192,9 +193,12 @@ class DesireHelper: return modifier - def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles, nudgeless_enabled): + def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles): self._update_nav_params() self.nav_desires_allowed = bool(getattr(starpilot_toggles, "nav_desires_allowed", self.nav_desires_allowed)) + self.nav_lane_positioning_allowed = bool( + getattr(starpilot_toggles, "nav_lane_positioning_allowed", self.nav_lane_positioning_allowed) + ) if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)): return log.Desire.none @@ -204,18 +208,20 @@ class DesireHelper: return log.Desire.none if modifier == "slightLeft": + if not self.nav_lane_positioning_allowed: + return log.Desire.none lane_change_direction = LaneChangeDirection.left desired_lane_width = starpilotPlan.laneWidthLeft - nudgeless_allowed = nudgeless_enabled and desired_lane_width >= starpilot_toggles.lane_detection_width if not carstate.rightBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction): - if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed: + if desired_lane_width >= starpilot_toggles.lane_detection_width and self._nav_torque_applied(carstate, lane_change_direction): return log.Desire.keepLeft elif modifier == "slightRight": + if not self.nav_lane_positioning_allowed: + return log.Desire.none lane_change_direction = LaneChangeDirection.right desired_lane_width = starpilotPlan.laneWidthRight - nudgeless_allowed = nudgeless_enabled and desired_lane_width >= starpilot_toggles.lane_detection_width if not carstate.leftBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction): - if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed: + if desired_lane_width >= starpilot_toggles.lane_detection_width and self._nav_torque_applied(carstate, lane_change_direction): return log.Desire.keepRight elif modifier in ("left", "sharpLeft"): turn_allowed = not carstate.rightBlinker and not carstate.leftBlindspot @@ -349,6 +355,6 @@ class DesireHelper: self.lane_change_wait_timer = 0.0 - nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles, nudgeless_enabled) + nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles) if nav_desire != log.Desire.none and self.lane_change_state == LaneChangeState.off: self.desire = nav_desire diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index 3b1f0ef54..cc6a9f3fd 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -33,6 +33,7 @@ def make_toggles(**overrides): "use_turn_desires": False, "lane_changes_require_cruise": False, "nav_desires_allowed": True, + "nav_lane_positioning_allowed": True, } defaults.update(overrides) return SimpleNamespace(**defaults) @@ -53,7 +54,7 @@ def test_nav_desires_keep_left_when_route_requests_it(): helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft"} helper.update( - make_car_state(vEgo=20.0), + make_car_state(vEgo=20.0, steeringPressed=True, steeringTorque=1.0), True, 0.0, make_plan(laneWidthLeft=4.2), @@ -74,7 +75,7 @@ def test_nav_desires_turn_right_below_lane_change_speed(): True, 0.0, make_plan(), - make_toggles(minimum_lane_change_speed=10.0), + make_toggles(minimum_lane_change_speed=10.0, nav_lane_positioning_allowed=False), ) assert helper.desire == log.Desire.turnRight @@ -110,7 +111,7 @@ def test_nav_desires_off_ramp_lane_guidance_becomes_keep_right(): } helper.update( - make_car_state(vEgo=22.5), + make_car_state(vEgo=22.5, steeringPressed=True, steeringTorque=-1.0), True, 0.0, make_plan(laneWidthRight=4.2), @@ -211,7 +212,7 @@ def test_nav_desires_wide_highway_edge_exit_lane_keeps_right(): } helper.update( - make_car_state(vEgo=19.0), + make_car_state(vEgo=19.0, steeringPressed=True, steeringTorque=-1.0), True, 0.0, make_plan(laneWidthRight=4.2), @@ -237,7 +238,7 @@ def test_nav_desires_shared_transition_lane_keeps_when_active_lane_is_not_outerm } helper.update( - make_car_state(vEgo=22.5), + make_car_state(vEgo=22.5, steeringPressed=True, steeringTorque=-1.0), True, 0.0, make_plan(laneWidthRight=4.2), @@ -261,7 +262,7 @@ def test_nav_desires_ambiguous_fork_slight_right_only_keeps_close_to_split(): } helper.update( - make_car_state(vEgo=22.5), + make_car_state(vEgo=22.5, steeringPressed=True, steeringTorque=-1.0), True, 0.0, make_plan(laneWidthRight=4.2), @@ -448,6 +449,16 @@ def test_nav_desires_nudgeless_only_when_engaged_blocks_keep_when_aol_only(): assert helper.desire == log.Desire.none + helper.update( + make_car_state(vEgo=20.0, steeringPressed=True, steeringTorque=-1.0), + True, + 0.0, + make_plan(laneWidthRight=4.2), + make_toggles(nav_desires_allowed=True, nav_lane_positioning_allowed=False, nudgeless=True), + ) + + assert helper.desire == log.Desire.none + def test_turn_desire_fires_below_lane_change_speed_when_no_stop(): helper = DesireHelper() @@ -512,11 +523,37 @@ def test_disabling_nav_desires_clears_active_route_desire_immediately(): helper = DesireHelper() helper._update_nav_params = lambda: None helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"} - car_state = make_car_state(vEgo=20.0) + car_state = make_car_state(vEgo=20.0, steeringPressed=True, steeringTorque=-1.0) plan = make_plan(laneWidthRight=4.2) - helper.update(car_state, True, 0.0, plan, make_toggles(nav_desires_allowed=True)) + helper.update(car_state, True, 0.0, plan, make_toggles(nav_desires_allowed=True, nav_lane_positioning_allowed=True)) assert helper.desire == log.Desire.keepRight - helper.update(car_state, True, 0.0, plan, make_toggles(nav_desires_allowed=False)) + helper.update(car_state, True, 0.0, plan, make_toggles(nav_desires_allowed=False, nav_lane_positioning_allowed=True)) + assert helper.desire == log.Desire.none + + +def test_nav_lane_positioning_requires_driver_confirmation(): + helper = DesireHelper() + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"} + + helper.update( + make_car_state(vEgo=20.0), + True, + 0.0, + make_plan(laneWidthRight=4.2), + make_toggles(nav_desires_allowed=True, nav_lane_positioning_allowed=True, nudgeless=True), + ) + + assert helper.desire == log.Desire.none + + helper.update( + make_car_state(vEgo=20.0, steeringPressed=True, steeringTorque=-1.0), + True, + 0.0, + make_plan(laneWidthRight=4.2), + make_toggles(nav_desires_allowed=True, nav_lane_positioning_allowed=False, nudgeless=True), + ) + assert helper.desire == log.Desire.none diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 78a2563de..89af5e1cf 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -50,6 +50,7 @@ SAFE_MODE_MANAGED_KEYS = ( "NNFFLite", "TurnDesires", "NavDesiresAllowed", + "NavLanePositioningAllowed", "NavLongitudinalAllowed", "QOLLateral", "PauseLateralSpeed", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 33e12e249..c0ca5ff62 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1074,6 +1074,7 @@ class StarPilotVariables: toggle.nnff = self.get_value("NNFF", condition=lateral_tuning and has_nnff and not is_angle_car) toggle.nnff_lite = self.get_value("NNFFLite", condition=not toggle.nnff and lateral_tuning and not is_angle_car) toggle.nav_desires_allowed = self.get_value("NavDesiresAllowed") + toggle.nav_lane_positioning_allowed = self.get_value("NavLanePositioningAllowed") toggle.use_turn_desires = self.get_value("TurnDesires", condition=lateral_tuning) lkas_button_control = self.get_button_function("LKASButtonControl", condition=toggle.car_make != "subaru") diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index e1276dd34..fad579e7b 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -269,8 +269,17 @@ }, { "key": "NavDesiresAllowed", - "label": "Use Route Desires", - "description": "Allow an active navigation route to request keep-left, keep-right, and low-speed turn desires.", + "label": "Use Route Desires for Turns", + "description": "Allow an active navigation route to request low-speed turn desires.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "LateralTune", + "settings_tier": "advanced" + }, + { + "key": "NavLanePositioningAllowed", + "label": "Use Route Desires for Lane Positioning", + "description": "Allow route lane bias only after steering torque confirms the requested direction. It never initiates a lane change by itself.", "data_type": "bool", "ui_type": "toggle", "parent_key": "LateralTune", diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index b596c3fed..395dbfc9e 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -90,7 +90,7 @@ def test_requested_simple_and_advanced_settings_tiers(): for key in ("AlwaysOnLateral", "LaneChanges", "QOLLateral"): assert lateral[key]["settings_tier"] == "simple" - for key in ("AdvancedLateralTune", "LateralTune", "NavDesiresAllowed"): + for key in ("AdvancedLateralTune", "LateralTune", "NavDesiresAllowed", "NavLanePositioningAllowed"): assert lateral[key]["settings_tier"] == "advanced" for key in ( @@ -122,6 +122,7 @@ def test_requested_simple_and_advanced_settings_tiers(): def test_hidden_feature_defaults_remain_enabled(): assert _declared_default("GalaxyDeveloperMode") == "0" assert _declared_default("NavDesiresAllowed") == "1" + assert _declared_default("NavLanePositioningAllowed") == "0" assert _declared_default("NavLongitudinalAllowed") == "1" for key in (