Toyota TSS2: custom longitudinal tuning (#1774)

* TSS2-Smooth: add param, flag injection, UI toggle, and metadata

- Register TSS2-Smooth param in params_keys.h
- Add TSS2-Smooth to toyota keys in initialize_params()
- Update opendbc submodule to use CP_SP flag instead of direct Params import
- Add TSS2 Smooth Longitudinal toggle to Toyota settings UI
- Disable toggle when stock longitudinal is enforced
- Add TSS2-Smooth entry to params_metadata.json

* toyota: rework TSS2 smooth param and toggle

* bump opendbc

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

.github/workflows/tests.yaml

@@ -50,6 +50,12 @@ jobs:
       timeout-minutes: 3
       run: TARGET_DIR=$STRIPPED_DIR tools/release/build_stripped.sh
     - run: ./tools/op.sh setup
+    - name: Restore scons cache
+      uses: actions/cache@v6
+      with:
+        path: /tmp/scons_cache
+        key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+        restore-keys: scons-${{ runner.os }}-${{ github.job }}-
     - name: Build openpilot and run checks
       timeout-minutes: 30
       working-directory: ${{ env.STRIPPED_DIR }}
@@ -89,6 +95,12 @@ jobs:
         FILTERED=$(echo "$PATH" | tr ':' '\n' | grep -v '/opt/homebrew' | tr '\n' ':')
         echo "PATH=${FILTERED}/usr/local/bin:/usr/bin:/bin:/usr/sbin:/sbin" >> $GITHUB_ENV
     - run: ./tools/op.sh setup
+    - name: Restore scons cache
+      uses: actions/cache@v6
+      with:
+        path: /tmp/scons_cache
+        key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+        restore-keys: scons-${{ runner.os }}-${{ github.job }}-
     - name: Building openpilot
       run: scons
 
@@ -121,6 +133,12 @@ jobs:
     - name: Install raylib headless dependencies
       if: ${{ !contains(runner.name, 'nsc') }}
       run: sudo apt-get install -y --no-install-recommends libgles2 libegl1
+    - name: Restore scons cache
+      uses: actions/cache@v6
+      with:
+        path: /tmp/scons_cache
+        key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+        restore-keys: scons-${{ runner.os }}-${{ github.job }}-
     - name: Build openpilot
       run: scons
     - name: Run unit tests
@@ -140,6 +158,12 @@ jobs:
     steps:
     - uses: actions/checkout@v7
     - run: ./tools/op.sh setup
+    - name: Restore scons cache
+      uses: actions/cache@v6
+      with:
+        path: /tmp/scons_cache
+        key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+        restore-keys: scons-${{ runner.os }}-${{ github.job }}-
     - name: Build openpilot
       run: scons
     - name: Run replay
@@ -228,6 +252,12 @@ jobs:
       - name: Install raylib headless dependencies
         if: ${{ !contains(runner.name, 'nsc') }}
         run: sudo apt-get install -y --no-install-recommends libgles2 libegl1
+      - name: Restore scons cache
+        uses: actions/cache@v6
+        with:
+          path: /tmp/scons_cache
+          key: scons-${{ runner.os }}-${{ github.job }}-${{ github.sha }}
+          restore-keys: scons-${{ runner.os }}-${{ github.job }}-
       - name: Build openpilot
         run: scons
       - name: Create UI Report

openpilot/common/params_keys.h

@@ -246,6 +246,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"TeslaMadsScreenButton", {PERSISTENT | BACKUP, INT, "0"}},
     {"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"ToyotaTSS2Smooth", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 

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/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/toyota.py

@@ -11,10 +11,12 @@ from openpilot.system.ui.lib.multilang import tr, tr_noop
 from openpilot.system.ui.widgets import DialogResult
 from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog
 from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
+from opendbc.car.toyota.values import ToyotaFlags
 
 
 ONROAD_ONLY_DESCRIPTION = tr_noop("Start the vehicle to check vehicle compatibility.")
 SNG_HACK_UNAVAILABLE = tr_noop("sunnypilot Longitudinal Control must be available and enabled for your vehicle to use this feature.")
+PLATFORM_UNAVAILABLE = tr_noop("This feature is currently not available on this platform.")
 
 DESCRIPTIONS = {
   'enforce_stock_longitudinal': tr_noop(
@@ -23,7 +25,11 @@ DESCRIPTIONS = {
   'stop_and_go_hack': tr_noop(
     'sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. ' +
     'This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk.'
-  )
+  ),
+  'tss2_smooth': tr_noop(
+    'Enable smoother longitudinal control for TSS2 vehicles. ' +
+    'Uses a custom speed-dependent integral gain curve for smoother traffic driving.'
+  ),
 }
 
 
@@ -47,9 +53,18 @@ class ToyotaSettings(BrandSettings):
       enabled=lambda: not ui_state.engaged,
     )
 
+    self.tss2_smooth = toggle_item_sp(
+      lambda: tr("TSS2 Smooth Longitudinal (Alpha)"),
+      description=lambda: tr(DESCRIPTIONS["tss2_smooth"]),
+      initial_state=ui_state.params.get_bool("ToyotaTSS2Smooth"),
+      callback=self._on_enable_tss2_smooth,
+      enabled=lambda: not ui_state.engaged,
+    )
+
     self.items = [
       self.enforce_stock_longitudinal,
       self.stop_and_go_hack,
+      self.tss2_smooth,
     ]
 
   def _on_enable_enforce_stock_longitudinal(self, state: bool):
@@ -61,6 +76,8 @@ class ToyotaSettings(BrandSettings):
             ui_state.params.put_bool("AlphaLongitudinalEnabled", False)
           ui_state.params.put_bool("ToyotaStopAndGoHack", False)
           self.stop_and_go_hack.action_item.set_state(False)
+          ui_state.params.put_bool("ToyotaTSS2Smooth", False)
+          self.tss2_smooth.action_item.set_state(False)
           ui_state.params.put_bool("OnroadCycleRequested", True)
         else:
           self.enforce_stock_longitudinal.action_item.set_state(False)
@@ -75,6 +92,25 @@ class ToyotaSettings(BrandSettings):
       ui_state.params.put_bool("ToyotaEnforceStockLongitudinal", False)
       ui_state.params.put_bool("OnroadCycleRequested", True)
 
+  def _on_enable_tss2_smooth(self, state: bool):
+    if state:
+      def confirm_callback(result: int):
+        if result == DialogResult.CONFIRM:
+          ui_state.params.put_bool("ToyotaTSS2Smooth", True)
+          ui_state.params.put_bool("OnroadCycleRequested", True)
+        else:
+          self.tss2_smooth.action_item.set_state(False)
+
+      content = (f"<h1>{self.tss2_smooth.title}</h1><br>" +
+                 f"<p>{self.tss2_smooth.description}</p>")
+
+      dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
+      gui_app.push_widget(dlg)
+
+    else:
+      ui_state.params.put_bool("ToyotaTSS2Smooth", False)
+      ui_state.params.put_bool("OnroadCycleRequested", True)
+
   def _on_enable_stop_and_go_hack(self, state: bool):
     if state:
       def confirm_callback(result: int):
@@ -113,9 +149,32 @@ class ToyotaSettings(BrandSettings):
         self.stop_and_go_hack.set_description(new_desc)
         if show_desc:
           self.stop_and_go_hack.show_description(True)
+
+      tss2 = bool(ui_state.CP.flags & ToyotaFlags.TSS2)
+      if longitudinal and not enforce_stock and tss2:
+        self.tss2_smooth.action_item.set_enabled(not ui_state.engaged)
+        new_desc = tr(DESCRIPTIONS["tss2_smooth"])
+        show_desc = False
+      else:
+        self.tss2_smooth.action_item.set_enabled(False)
+        self.tss2_smooth.action_item.set_state(False)
+        unavailable = SNG_HACK_UNAVAILABLE if tss2 else PLATFORM_UNAVAILABLE
+        new_desc = "<b>" + tr(unavailable) + "</b>\n\n" + tr(DESCRIPTIONS["tss2_smooth"])
+        show_desc = True
+
+      if self.tss2_smooth.description != new_desc:
+        self.tss2_smooth.set_description(new_desc)
+        if show_desc:
+          self.tss2_smooth.show_description(True)
     else:
       self.stop_and_go_hack.action_item.set_enabled(False)
       new_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["stop_and_go_hack"])
       if self.stop_and_go_hack.description != new_desc:
         self.stop_and_go_hack.set_description(new_desc)
         self.stop_and_go_hack.show_description(True)
+
+      self.tss2_smooth.action_item.set_enabled(False)
+      new_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["tss2_smooth"])
+      if self.tss2_smooth.description != new_desc:
+        self.tss2_smooth.set_description(new_desc)
+        self.tss2_smooth.show_description(True)

openpilot/sunnypilot/selfdrive/car/interfaces.py

@@ -126,6 +126,7 @@ def initialize_params(params) -> list[dict[str, Any]]:
   keys.extend([
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaTSS2Smooth",
   ])
 
   return [{k: params.get(k, return_default=True)} for k in keys]

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

openpilot/sunnypilot/sunnylink/capabilities.py

@@ -10,6 +10,7 @@ from openpilot.cereal import custom, messaging
 from opendbc.car.structs import car
 from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, UNSUPPORTED_LONGITUDINAL_CAR
 from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaFlags
 from opendbc.sunnypilot.car.interfaces import has_bsm
 from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
 from openpilot.common.params import Params
@@ -43,6 +44,7 @@ CAPABILITY_FIELDS = (
   "device_type",
   "subaru_has_sng",
   "hyundai_alpha_long_available",
+  "toyota_tss2",
 )
 
 CAPABILITY_LABELS: dict[str, str] = {
@@ -65,6 +67,7 @@ CAPABILITY_LABELS: dict[str, str] = {
   "device_type": "Device type",
   "subaru_has_sng": "Subaru Stop-and-Go available",
   "hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available",
+  "toyota_tss2": "Toyota TSS2 platform",
 }
 
 # Explicit defaults for non-boolean capability fields
@@ -111,6 +114,15 @@ def _resolve_brand_capabilities(caps: dict, bundle_platform: str, CP) -> None:
       caps["subaru_has_sng"] = not bool(CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID))
       caps["has_stop_and_go"] = caps["subaru_has_sng"]
 
+  elif brand == "toyota":
+    if bundle_platform:
+      try:
+        caps["toyota_tss2"] = bool(TOYOTA_CAR[bundle_platform].config.flags & ToyotaFlags.TSS2)
+      except KeyError:
+        cloudlog.exception(f"capabilities: unknown toyota platform {bundle_platform!r}")
+    elif CP is not None:
+      caps["toyota_tss2"] = bool(CP.flags & ToyotaFlags.TSS2)
+
 
 def generate_capabilities(params: Params | None = None) -> dict:
   """Generate a SettingsCapabilities dict from CarParams + boolean params.

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -2324,6 +2324,33 @@
               "equals": false
             }
           ]
+        },
+        {
+          "key": "ToyotaTSS2Smooth",
+          "widget": "toggle",
+          "needs_onroad_cycle": true,
+          "title": "TSS2 Smooth Longitudinal (Alpha)",
+          "description": "Enable smoother longitudinal control for TSS2 vehicles. Uses a custom speed-dependent integral gain curve for smoother traffic driving.",
+          "enablement": [
+            {
+              "type": "not_engaged"
+            },
+            {
+              "type": "capability",
+              "field": "has_longitudinal_control",
+              "equals": true
+            },
+            {
+              "type": "param",
+              "key": "ToyotaEnforceStockLongitudinal",
+              "equals": false
+            },
+            {
+              "type": "capability",
+              "field": "toyota_tss2",
+              "equals": true
+            }
+          ]
         }
       ]
     }

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -101,3 +101,18 @@ sections:
     - type: param
       key: ToyotaEnforceStockLongitudinal
       equals: false
+  - key: ToyotaTSS2Smooth
+    widget: toggle
+    needs_onroad_cycle: true
+    title: TSS2 Smooth Longitudinal (Alpha)
+    description: Enable smoother longitudinal control for TSS2 vehicles. Uses a custom speed-dependent integral gain curve
+      for smoother traffic driving.
+    enablement:
+    - $ref: '#/macros/not_engaged'
+    - $ref: '#/macros/longitudinal'
+    - type: param
+      key: ToyotaEnforceStockLongitudinal
+      equals: false
+    - type: capability
+      field: toyota_tss2
+      equals: true

openpilot/sunnypilot/sunnylink/tests/test_capabilities.py

@@ -75,6 +75,12 @@ class TestOpaquePerBrandFlags(OpenpilotTestCase):
   def test_hyundai_alpha_long_available_default_false(self, caps):
     assert caps["hyundai_alpha_long_available"] is False
 
+  def test_toyota_tss2_field_present(self):
+    assert "toyota_tss2" in CAPABILITY_FIELDS
+
+  def test_toyota_tss2_default_false(self, caps):
+    assert caps["toyota_tss2"] is False
+
 
 class TestCapabilitiesShape(OpenpilotTestCase):
   def test_all_fields_present(self, caps):

openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py

@@ -207,6 +207,7 @@ class TestNotEngagedReplacement(OpenpilotTestCase):
     "AlphaLongitudinalEnabled",
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
+    "ToyotaTSS2Smooth",
   ], names=["key"])
   def test_offroad_only_replaced_with_not_engaged(self, schema, key):
     """These items should use not_engaged, not offroad_only."""

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -286,6 +286,7 @@ class TestKnownVehicleSettings(OpenpilotTestCase):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))}
     assert "ToyotaEnforceStockLongitudinal" in keys
     assert "ToyotaStopAndGoHack" in keys
+    assert "ToyotaTSS2Smooth" in keys
 
   def test_tesla_has_coop_steering(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("tesla"))}