NudgeMe

common/params_keys.h

@@ -432,6 +432,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"NoLogging", {PERSISTENT, BOOL, "0", "0", 2}},
     {"NoUploads", {PERSISTENT, BOOL, "0", "0", 2}},
     {"NudgelessLaneChange", {PERSISTENT, BOOL, "0", "0", 0}},
+    {"NudgelessLaneChangeOnlyWhenEngaged", {PERSISTENT, BOOL, "0", "0", 1}},
     {"NumericalTemp", {PERSISTENT, BOOL, "1", "0", 3}},
     {"Offset1", {PERSISTENT, FLOAT, "5.0", "0.0", 0}},
     {"Offset2", {PERSISTENT, FLOAT, "5.0", "0.0", 0}},

selfdrive/controls/lib/desire_helper.py

@@ -119,6 +119,13 @@ class DesireHelper:
 
     return distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS))
 
+  @staticmethod
+  def _nudgeless_enabled(starpilot_toggles, controls_enabled):
+    nudgeless = bool(getattr(starpilot_toggles, "nudgeless", False))
+    if getattr(starpilot_toggles, "nudgeless_lane_change_only_when_engaged", False):
+      nudgeless &= bool(controls_enabled)
+    return nudgeless
+
   @staticmethod
   def _nav_keep_is_imminent(carstate, maneuver_distance, maneuver_type="", same_side_lane_count=0):
     try:
@@ -172,7 +179,7 @@ class DesireHelper:
 
     return modifier
 
-  def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles):
+  def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles, nudgeless_enabled):
     self._update_nav_params()
     if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)):
       return log.Desire.none
@@ -185,14 +192,14 @@ class DesireHelper:
     if modifier == "slightLeft":
       lane_change_direction = LaneChangeDirection.left
       desired_lane_width = starpilotPlan.laneWidthLeft
-      nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width
+      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:
           return log.Desire.keepLeft
     elif modifier == "slightRight":
       lane_change_direction = LaneChangeDirection.right
       desired_lane_width = starpilotPlan.laneWidthRight
-      nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width
+      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:
           return log.Desire.keepRight
@@ -209,11 +216,13 @@ class DesireHelper:
   def get_lane_change_direction(CS):
     return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
 
-  def update(self, carstate, lateral_active, lane_change_prob, starpilotPlan, starpilot_toggles):
+  def update(self, carstate, lateral_active, lane_change_prob, starpilotPlan, starpilot_toggles, controls_enabled=None):
     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)
+    controls_enabled = bool(getattr(cruise_state, "enabled", False)) if controls_enabled is None else bool(controls_enabled)
+    nudgeless_enabled = self._nudgeless_enabled(starpilot_toggles, controls_enabled)
     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))
 
@@ -244,7 +253,7 @@ class DesireHelper:
         if torque_applied:
           self.lane_change_wait_timer = starpilot_toggles.lane_change_delay
         else:
-          torque_applied |= starpilot_toggles.nudgeless
+          torque_applied |= nudgeless_enabled
           torque_applied &= self.lane_change_wait_timer >= starpilot_toggles.lane_change_delay
 
           desired_lane_width = starpilotPlan.laneWidthLeft if self.lane_change_direction == LaneChangeDirection.left else starpilotPlan.laneWidthRight
@@ -312,6 +321,6 @@ class DesireHelper:
 
       self.lane_change_wait_timer = 0.0
 
-    nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles)
+    nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles, nudgeless_enabled)
     if nav_desire != log.Desire.none and self.lane_change_state == LaneChangeState.off:
       self.desire = nav_desire

selfdrive/controls/tests/test_navigation_desires.py

@@ -28,6 +28,7 @@ def make_toggles(**overrides):
     "lane_detection_width": 3.0,
     "minimum_lane_change_speed": 10.0,
     "nudgeless": True,
+    "nudgeless_lane_change_only_when_engaged": False,
     "one_lane_change": False,
     "use_turn_desires": False,
     "lane_changes_require_cruise": False,
@@ -354,6 +355,71 @@ def test_lane_changes_without_cruise_requirement_keep_existing_behavior():
   assert helper.lane_change_direction == LaneChangeDirection.left
 
 
+def test_nudgeless_only_when_engaged_allows_automatic_lane_change_when_engaged():
+  helper = DesireHelper()
+
+  for _ in range(2):
+    helper.update(
+      make_car_state(leftBlinker=True),
+      True,
+      0.0,
+      make_plan(),
+      make_toggles(nudgeless_lane_change_only_when_engaged=True),
+      controls_enabled=True,
+    )
+
+  assert helper.lane_change_state == LaneChangeState.laneChangeStarting
+  assert helper.lane_change_direction == LaneChangeDirection.left
+
+
+def test_nudgeless_only_when_engaged_requires_nudge_when_aol_only():
+  helper = DesireHelper()
+  toggles = make_toggles(nudgeless_lane_change_only_when_engaged=True)
+
+  for _ in range(2):
+    helper.update(
+      make_car_state(leftBlinker=True),
+      True,
+      0.0,
+      make_plan(),
+      toggles,
+      controls_enabled=False,
+    )
+
+  assert helper.lane_change_state == LaneChangeState.preLaneChange
+  assert helper.lane_change_direction == LaneChangeDirection.left
+
+  helper.update(
+    make_car_state(leftBlinker=True, steeringPressed=True, steeringTorque=1.0),
+    True,
+    0.0,
+    make_plan(),
+    toggles,
+    controls_enabled=False,
+  )
+
+  assert helper.lane_change_state == LaneChangeState.laneChangeStarting
+  assert helper.lane_change_direction == LaneChangeDirection.left
+
+
+def test_nav_desires_nudgeless_only_when_engaged_blocks_keep_when_aol_only():
+  helper = DesireHelper()
+  helper.nav_desires_allowed = True
+  helper._update_nav_params = lambda: None
+  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft"}
+
+  helper.update(
+    make_car_state(vEgo=20.0),
+    True,
+    0.0,
+    make_plan(laneWidthLeft=4.2),
+    make_toggles(nudgeless=True, nudgeless_lane_change_only_when_engaged=True),
+    controls_enabled=False,
+  )
+
+  assert helper.desire == log.Desire.none
+
+
 def test_nav_desires_disabled_leave_desire_unchanged():
   helper = DesireHelper()
   helper.nav_desires_allowed = False

selfdrive/modeld/modeld.py

@@ -699,7 +699,7 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['starpilotPlan'], starpilot_toggles)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['starpilotPlan'], starpilot_toggles, sm['carControl'].enabled)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       starpilot_modelv2_send.starpilotModelV2.turnDirection = DH.turn_direction

selfdrive/modeld/modeld_v16.py

@@ -479,7 +479,7 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      desire_helper.update(sm["carState"], sm["carControl"].latActive, lane_change_prob, sm["starpilotPlan"], starpilot_toggles)
+      desire_helper.update(sm["carState"], sm["carControl"].latActive, lane_change_prob, sm["starpilotPlan"], starpilot_toggles, sm["carControl"].enabled)
       modelv2_send.modelV2.meta.laneChangeState = desire_helper.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = desire_helper.lane_change_direction
       starpilot_modelv2_send.starpilotModelV2.turnDirection = desire_helper.turn_direction

starpilot/common/safe_mode.py

@@ -37,6 +37,7 @@ SAFE_MODE_MANAGED_KEYS = (
   "LaneDetectionWidth",
   "MinimumLaneChangeSpeed",
   "NudgelessLaneChange",
+  "NudgelessLaneChangeOnlyWhenEngaged",
   "OneLaneChange",
   "NNFF",
   "NNFFLite",

starpilot/common/starpilot_variables.py

@@ -967,6 +967,7 @@ class StarPilotVariables:
     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)
     toggle.nudgeless = self.get_value("NudgelessLaneChange", condition=toggle.lane_changes)
+    toggle.nudgeless_lane_change_only_when_engaged = self.get_value("NudgelessLaneChangeOnlyWhenEngaged", condition=toggle.lane_changes and toggle.nudgeless)
     toggle.one_lane_change = self.get_value("OneLaneChange", condition=toggle.lane_changes)
 
     # Lane change pace: 1 = smoothest (~8 s target), 10 = stock (no clamp applied)

starpilot/system/the_pond/assets/components/tools/device_settings_layout.json

@@ -137,6 +137,14 @@
         "ui_type": "toggle",
         "parent_key": "LaneChanges"
       },
+      {
+        "key": "NudgelessLaneChangeOnlyWhenEngaged",
+        "label": "Automatic Lane Changes Only When Engaged",
+        "description": "Require a steering-wheel nudge for lane changes while using Always On Lateral without full engagement.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "LaneChanges"
+      },
       {
         "key": "LaneChangeTime",
         "label": "Lane Change Delay",

tools/StarPilot/feasibleparams.txt

@@ -203,6 +203,7 @@ NoLogging
 NoUploads
 NostalgiaMode
 NudgelessLaneChange
+NudgelessLaneChangeOnlyWhenEngaged
 NumericalTemp
 Offroad_ExcessiveActuation
 Offset1