Tunes and Bugfixes

common/params_keys.h

@@ -744,6 +744,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"SubaruAvhStartup", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"SubaruRedneckCruise", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}},
+    {"TeslaAOLDisengageOnBrake", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
     {"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
     {"TetheringEnabled", {PERSISTENT, INT, "0", "0", 0}},

opendbc_repo/opendbc/car/hyundai/carcontroller.py

@@ -834,7 +834,7 @@ class CarController(CarControllerBase):
                       not CS.out.gasPressed and not CS.out.brakePressed)
       if pedal_active:
         set_speed = hud_control.setSpeed
-        if not np.isfinite(set_speed) or not 1.0 <= set_speed <= 40.0:
+        if not np.isfinite(set_speed) or set_speed < 1.0:
           self._ray_pedal_gas_last = 0.0
         else:
           speed_error = set_speed - CS.out.vEgo

opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py

@@ -1086,6 +1086,31 @@ class TestHyundaiFingerprint:
 
     assert not (FPCP.flags & HyundaiStarPilotFlags.HAS_LKAS12)
 
+  @pytest.mark.parametrize("length, expected", ((6, True), (8, False)))
+  def test_stinger_only_replaces_six_byte_lkas12(self, length, expected):
+    fingerprint = gen_empty_fingerprint()
+    fingerprint[2][0x53E] = length
+    CP = CarInterface.get_params(CAR.KIA_STINGER_2022, fingerprint, [], True, False, False, None)
+    FPCP = CarInterface.get_starpilot_params(CAR.KIA_STINGER_2022, fingerprint, [], CP, get_test_toggles())
+
+    assert bool(FPCP.flags & HyundaiStarPilotFlags.HAS_LKAS12) is expected
+
+  @pytest.mark.parametrize("alpha_long, main_aol, expected", (
+    (True, True, True), (True, False, True), (False, True, False),
+  ))
+  def test_stinger_aol_latches_lkas_after_long_engagement(self, alpha_long, main_aol, expected):
+    toggles = get_test_toggles()
+    toggles.always_on_lateral_main = main_aol
+    fingerprint = gen_empty_fingerprint()
+    CP = CarInterface.get_params(CAR.KIA_STINGER_2022, fingerprint, [], alpha_long, False, False, toggles)
+    FPCP = CarInterface.get_starpilot_params(CAR.KIA_STINGER_2022, fingerprint, [], CP, toggles)
+
+    assert bool(FPCP.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE) is expected
+
+    sonata_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], alpha_long, False, False, toggles)
+    sonata_fpcp = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA, fingerprint, [], sonata_cp, toggles)
+    assert not (sonata_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE)
+
   def test_ray_ev_does_not_treat_eight_byte_485_as_lfa(self):
     fingerprint = gen_empty_fingerprint()
     fingerprint[2][0x485] = 8

opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py

@@ -235,6 +235,12 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed):
   CS.out.vEgo = 12.0
   hud.setSpeed = 8.0 / 3.6
   assert pedal_msg(-0.3, 472)[:4] == bytes(4)
+  hud.setSpeed = 145.0 / 3.6
+  assert pedal_msg(1.5, 476)[4] & 0x80
+  assert controller._ray_pedal_gas_last == pytest.approx(0.02)
+  assert pedal_msg(1.5, 480)[4] & 0x80
+  assert controller._ray_pedal_gas_last == pytest.approx(0.04)
+  assert pedal_msg(-0.3, 484)[:4] == bytes(4)
 
 
 @pytest.mark.parametrize("candidate", [CAR.KIA_RAY_EV, CAR.HYUNDAI_KONA_EV_NON_SCC])

opendbc_repo/opendbc/car/interfaces.py

@@ -244,7 +244,8 @@ class CarInterfaceBase(ABC):
           if 0x1FA in fingerprint[CAN.ECAN]:
             fp_ret.flags |= HyundaiStarPilotFlags.SPEED_LIMIT_AVAILABLE.value
 
-        if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2]:
+        if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2] and \
+            (candidate != HYUNDAI.KIA_STINGER_2022 or fingerprint[2][0x53E] == 6):
           fp_ret.flags |= HyundaiStarPilotFlags.HAS_LKAS12.value
 
         fp_ret.redneckCruiseAvailable = (bool(CP.flags & HyundaiFlags.NON_SCC) and
@@ -275,6 +276,9 @@ class CarInterfaceBase(ABC):
           if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID:
             fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
 
+        if candidate == HYUNDAI.KIA_STINGER_2022 and CP.openpilotLongitudinalControl:
+          fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
+
         # The refresh Elantra's safety mapping comes from the resolved Galaxy
         # toggle above, not from this legacy persisted-parameter fallback.
         if candidate != HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and \

opendbc_repo/opendbc/safety/tests/test_hyundai.py

@@ -635,6 +635,23 @@ class TestHyundaiLongitudinalAolLkasOnEngageSafety(HyundaiAolLkasOnEngageBase, T
                                  HyundaiSafetyFlags.LONG | HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE)
     self.safety.init_tests()
 
+  def test_main_off_after_brake_keeps_lateral_permission(self):
+    self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
+
+    self._rx(self._button_msg(Buttons.NONE, main_button=1))
+    self._rx(self._button_msg(Buttons.NONE, main_button=0))
+    self._rx(self._button_msg(Buttons.SET))
+    self._rx(self._button_msg(Buttons.NONE))
+    self._rx(self._user_brake_msg(True))
+    self._rx(self._button_msg(Buttons.NONE, main_button=1))
+    self._rx(self._button_msg(Buttons.NONE, main_button=0))
+
+    self.assertFalse(self.safety.get_controls_allowed())
+    self.assertFalse(self.safety.get_acc_main_on())
+    self.assertTrue(self.safety.get_lkas_on())
+    self._set_prev_torque(0)
+    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
+
 
 class TestHyundaiLongitudinalAolMainLkasOnEngageSafety(TestHyundaiLongitudinalSafety):
   def setUp(self):

selfdrive/controls/controlsd.py

@@ -31,6 +31,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from openpilot.selfdrive.controls.lib.steering_saturation import is_angle_steering_limited
 from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
+from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import GENESIS_GV70_CARS, GenesisGV70HighwayCommandStabilizer
 from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   BOLT_2018_2021_STEER_RATIO_TEST_SCALE,
   LatControlTorque,
@@ -426,6 +427,9 @@ class Controls:
       self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
     elif self.CP.lateralTuning.which() == 'torque':
       self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL)
+    self.gv70_highway_stabilizer = (GenesisGV70HighwayCommandStabilizer()
+                                    if self.CP.carFingerprint in GENESIS_GV70_CARS and self.CP.lateralTuning.which() == 'torque'
+                                    else None)
 
     self.sm = self.sm.extend(['liveDelay', 'starpilotCarState', 'starpilotPlan'])
 
@@ -747,6 +751,15 @@ class Controls:
       bool(CS.leftBlinker or CS.rightBlinker),
       bool(CS.steeringPressed))
 
+    if self.gv70_highway_stabilizer is not None:
+      stabilize_gv70 = (CC.latActive and isinstance(self.LaC, LatControlTorque) and
+                       not CS.steeringPressed and not CS.leftBlinker and not CS.rightBlinker and
+                       not self.starpilot_toggles.lane_centering and
+                       model_v2.meta.laneChangeState == LaneChangeState.off and
+                       self.sm.all_checks(['modelV2']))
+      new_desired_curvature = self.gv70_highway_stabilizer.update(
+        new_desired_curvature, CS.vEgo, stabilize_gv70, DT_CTRL)
+
     jerk_factor = 1.0
     if self.starpilot_toggles.lane_change_pace < 10:
       set_jerk = self.starpilot_toggles.lane_change_jerk_factor

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -1,4 +1,5 @@
 import ast
+from collections import deque
 import json
 import math
 import numpy as np
@@ -274,6 +275,14 @@ GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04
 GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08
 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55
 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.065
+GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP = [40.0 * CV.MPH_TO_MS, 50.0 * CV.MPH_TO_MS]
+GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [0.45, 0.65]
+GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC = 0.85
+GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC = 0.35
+GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT = 0.06
+GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_WINDOW = 4.0
+GENESIS_GV70_HIGHWAY_STABILIZER_REDUCTION = 0.70
+GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA = 0.20
 
 GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10
 GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION = 0.50
@@ -3291,6 +3300,57 @@ def get_genesis_gv70_stabilized_output(output_torque: float, prev_output_torque:
   return float(output_torque + speed_weight * (smoothed_output - output_torque))
 
 
+class GenesisGV70HighwayCommandStabilizer:
+  def __init__(self) -> None:
+    self.reset()
+
+  def reset(self) -> None:
+    self.baseline: float | None = None
+    self.last_sign = 0
+    self.reversals: deque[float] = deque()
+    self.elapsed = 0.0
+    self.blend = 0.0
+
+  def update(self, curvature: float, v_ego: float, enabled: bool, dt: float) -> float:
+    if not enabled or v_ego <= GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP[0] or not math.isfinite(curvature):
+      self.reset()
+      return curvature
+
+    self.elapsed += dt
+    lateral_accel = curvature * v_ego ** 2
+    if self.baseline is None:
+      self.baseline = lateral_accel
+    self.baseline += dt / (GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC + dt) * (lateral_accel - self.baseline)
+    residual = lateral_accel - self.baseline
+
+    if abs(lateral_accel) >= GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP[1]:
+      self.last_sign = 0
+      self.reversals.clear()
+      self.blend = 0.0
+      return curvature
+
+    sign = 0
+    if residual > GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT:
+      sign = 1
+    elif residual < -GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT:
+      sign = -1
+    if sign and sign != self.last_sign:
+      if self.last_sign:
+        self.reversals.append(self.elapsed)
+      self.last_sign = sign
+    while self.reversals and self.elapsed - self.reversals[0] > GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_WINDOW:
+      self.reversals.popleft()
+
+    speed_weight = float(np.interp(v_ego, GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP, [0.0, 1.0]))
+    target_blend = speed_weight if len(self.reversals) >= 3 else 0.0
+    self.blend += dt / (GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC + dt) * (target_blend - self.blend)
+    center_weight = float(np.interp(abs(lateral_accel), GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP, [1.0, 0.0]))
+    correction = float(np.clip(GENESIS_GV70_HIGHWAY_STABILIZER_REDUCTION * residual,
+                               -GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA,
+                               GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA))
+    return float((lateral_accel - self.blend * center_weight * correction) / v_ego ** 2)
+
+
 def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0,
                                        desired_lateral_jerk: float = 0.0) -> float:
   base_threshold = get_standard_friction_threshold(v_ego)

selfdrive/controls/lib/longcontrol.py

@@ -161,7 +161,7 @@ class LongControl:
     if not preserve_stop_release:
       self.stop_release_counter = 0
 
-  def _stop_release_ready(self, CS, a_target, should_stop, has_lead, starpilot_toggles):
+  def _stop_release_ready(self, CS, a_target, should_stop, has_lead, starpilot_toggles, leads=None):
     if self.long_control_state != LongCtrlState.stopping:
       self.stop_release_counter = 0
       return True
@@ -170,6 +170,10 @@ class LongControl:
       self.stop_release_counter = 0
       return False
 
+    if self.vehicle_tuning.hold_toyota_corolla_for_stopped_lead(CS.vEgo, leads):
+      self.stop_release_counter = 0
+      return False
+
     if CS.vEgo > starpilot_toggles.vEgoStarting:
       self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
       return True
@@ -247,7 +251,9 @@ class LongControl:
       )
 
     previous_long_control_state = self.long_control_state
-    allow_stopping_release = self._stop_release_ready(CS, a_target, should_stop, has_lead, starpilot_toggles)
+    allow_stopping_release = self._stop_release_ready(
+      CS, a_target, should_stop, has_lead, starpilot_toggles, leads=leads,
+    )
     self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill, starpilot_toggles,

selfdrive/controls/lib/longcontrol_vehicle_tunes.py

@@ -46,6 +46,9 @@ TOYOTA_COROLLA_TARGET_FILTER_UP_TAU = 0.30
 TOYOTA_COROLLA_TARGET_FILTER_DOWN_TAU = 0.18
 TOYOTA_COROLLA_TARGET_FILTER_BRAKE_BYPASS = -0.75
 TOYOTA_COROLLA_TARGET_FILTER_DROP_BYPASS = 0.45
+TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_EGO_SPEED = 0.5
+TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_DISTANCE = 8.0
+TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_SPEED = 0.35
 VOLT_CRUISE_INTEGRATOR_MIN_SPEED = 8.0
 VOLT_CRUISE_INTEGRATOR_TARGET_MAX = 0.12
 VOLT_CRUISE_INTEGRATOR_ERROR_MAX = 0.12
@@ -453,6 +456,18 @@ class LongControlVehicleTuning:
     self.toyota_corolla_filtered_a_target += alpha * (float(a_target) - self.toyota_corolla_filtered_a_target)
     return self.toyota_corolla_filtered_a_target
 
+  def hold_toyota_corolla_for_stopped_lead(self, v_ego, leads=None):
+    if not self.is_toyota_corolla_tss2 or v_ego > TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_EGO_SPEED:
+      return False
+
+    return any(
+      bool(getattr(lead, "status", False)) and
+      0.0 < float(getattr(lead, "dRel", 0.0)) <= TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_DISTANCE and
+      abs(float(getattr(lead, "yRel", 0.0))) <= 1.75 and
+      abs(float(getattr(lead, "vLead", 0.0))) <= TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_SPEED
+      for lead in (leads or ())
+    )
+
   def get_integrator_freeze(self, last_output_accel, a_target, error, v_ego, accel_limits):
     volt_test_tune_handoff = self.is_volt and testing_ground.use_2
 

selfdrive/controls/tests/test_gv70_highway_stabilizer.py

@@ -0,0 +1,63 @@
+import math
+
+import numpy as np
+import pytest
+
+from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
+from openpilot.common.constants import CV
+from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
+  GENESIS_GV70_CARS,
+  GenesisGV70HighwayCommandStabilizer,
+)
+
+
+def update_accel(stabilizer: GenesisGV70HighwayCommandStabilizer, lateral_accel: float,
+                 speed: float = 30.0, enabled: bool = True) -> float:
+  return stabilizer.update(lateral_accel / speed ** 2, speed, enabled, 0.01) * speed ** 2
+
+
+def test_only_electrified_gv70_selected():
+  assert GENESIS_GV70_CARS == (HYUNDAI_CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,)
+  assert HYUNDAI_CAR.GENESIS_G70_2020 not in GENESIS_GV70_CARS
+
+
+@pytest.mark.parametrize('speed', [10.0, 30.0 * CV.MPH_TO_MS, 40.0 * CV.MPH_TO_MS])
+def test_no_change_at_low_speed(speed):
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  for i in range(1200):
+    accel = 0.3 * math.sin(2.0 * math.pi * 0.5 * i * 0.01)
+    assert update_accel(stabilizer, accel, speed) == pytest.approx(accel)
+
+
+def test_repeated_highway_reversals_are_bounded_and_damped():
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  raw, shaped = [], []
+  for i in range(1200):
+    accel = 0.15 + 0.3 * math.sin(2.0 * math.pi * 0.5 * i * 0.01)
+    raw.append(accel)
+    shaped.append(update_accel(stabilizer, accel))
+
+  assert np.std(shaped[600:]) < 0.75 * np.std(raw[600:])
+  assert np.max(np.abs(np.array(raw) - np.array(shaped))) <= 0.20 + 1e-6
+
+
+def test_sustained_curve_is_unchanged():
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  curve = np.concatenate((np.linspace(0.0, 0.55, 150), np.full(300, 0.55), np.linspace(0.55, 0.0, 150)))
+  for accel in curve:
+    assert update_accel(stabilizer, float(accel)) == pytest.approx(accel)
+
+
+def test_strong_turn_and_driver_input_reset_stabilizer():
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  for i in range(1000):
+    accel = 0.3 * math.sin(2.0 * math.pi * 0.5 * i * 0.01)
+    update_accel(stabilizer, accel)
+
+  for _ in range(100):
+    assert update_accel(stabilizer, 0.8) == pytest.approx(0.8)
+  for _ in range(100):
+    assert update_accel(stabilizer, 0.4) == pytest.approx(0.4)
+
+  assert update_accel(stabilizer, -0.3, enabled=False) == pytest.approx(-0.3)
+  assert update_accel(stabilizer, 0.3) == pytest.approx(0.3)

selfdrive/controls/tests/test_longcontrol.py

@@ -558,6 +558,66 @@ def test_update_releases_stopping_on_small_sustained_positive_target():
   assert lc.long_control_state == LongCtrlState.starting
 
 
+def test_corolla_holds_stop_until_close_lead_moves():
+  CP = make_longcontrol_cp(
+    brand="toyota",
+    carFingerprint=TOYOTA_CAR.TOYOTA_COROLLA_TSS2,
+  )
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.stopping
+  CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
+  CS.cruiseState.standstill = False
+  lead = SimpleNamespace(status=True, dRel=6.2, yRel=0.0, vLead=0.1)
+
+  for _ in range(40):
+    output_accel = lc.update(
+      active=True,
+      CS=CS,
+      a_target=0.18,
+      should_stop=False,
+      accel_limits=(-3.0, 2.0),
+      starpilot_toggles=make_toggles(),
+      has_lead=True,
+      leads=(lead, None),
+    )
+    assert lc.long_control_state == LongCtrlState.stopping
+    assert output_accel <= 0.0
+
+  lead.vLead = 0.6
+  lc.update(
+    active=True,
+    CS=CS,
+    a_target=0.18,
+    should_stop=False,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(),
+    has_lead=True,
+    leads=(lead, None),
+  )
+  assert lc.long_control_state == LongCtrlState.pid
+
+
+def test_non_corolla_releases_stop_with_stopped_lead_as_before():
+  CP = make_longcontrol_cp(brand="honda")
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.stopping
+  CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
+  CS.cruiseState.standstill = False
+  lead = SimpleNamespace(status=True, dRel=6.2, yRel=0.0, vLead=0.1)
+
+  lc.update(
+    active=True,
+    CS=CS,
+    a_target=0.18,
+    should_stop=False,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(),
+    has_lead=True,
+    leads=(lead, None),
+  )
+  assert lc.long_control_state == LongCtrlState.pid
+
+
 def test_corolla_tss2_stop_release_ramps_positive_target():
   CP = make_longcontrol_cp(
     brand="toyota",

selfdrive/controls/tests/test_starpilot_planner.py

@@ -6,6 +6,7 @@ import pytest
 
 from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.drive_helpers import get_lateral_active
 from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner, get_force_stop_jerk_scale
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_hyundai_canfd_scc_jerk_limits,
@@ -162,6 +163,19 @@ def test_standstill_without_turn_signal_keeps_lateral_allowed(monkeypatch):
     planner.shutdown()
 
 
+@pytest.mark.parametrize("left_blinker", [False, True])
+def test_pause_steering_below_speed_includes_standstill(monkeypatch, left_blinker):
+  planner = make_planner(monkeypatch)
+
+  try:
+    toggles = make_toggles(pause_lateral_below_speed=35.0 * CV.MPH_TO_MS, pause_lateral_below_signal=False)
+    planner.update(0.0, False, make_sm(planner, frame=1, v_ego=0.0, left_blinker=left_blinker, standstill=True), toggles)
+    assert planner.lateral_check is False
+    assert not get_lateral_active(False, False, True, False, False, True, True, planner.lateral_check)
+  finally:
+    planner.shutdown()
+
+
 def test_manual_lateral_pause_blocks_lateral_while_cruise_is_enabled(monkeypatch):
   planner = make_planner(monkeypatch)
 

selfdrive/ui/layouts/settings/starpilot/lateral.py

@@ -117,6 +117,13 @@ class StarPilotLateralLayout(_SettingsPage):
 
     # ── 1. Steering Behavior ──
     self._behavior_rows = [
+      SettingRow(
+        "TeslaAOLDisengageOnBrake", "toggle", tr_noop("Disengage AOL on Brake"),
+        subtitle=tr_noop("Keep steering off after pressing the brake until openpilot is engaged again."),
+        get_state=lambda: p.get_bool("TeslaAOLDisengageOnBrake"),
+        set_state=lambda s: p.put_bool("TeslaAOLDisengageOnBrake", s),
+        visible=lambda: aol_on() and cs.isTesla,
+      ),
       SettingRow(
         "PauseAOLOnBrake", "value", tr_noop("Pause AOL On Brake"),
         subtitle=tr_noop("Pause AOL below this speed while brake is pressed."),

selfdrive/ui/lib/starpilot_state.py

@@ -20,6 +20,7 @@ class StarPilotCarState:
     isJeep: bool = False
     isToyota: bool = False
     isSubaru: bool = False
+    isTesla: bool = False
     isVolt: bool = False
     isBolt: bool = False
     isAngleCar: bool = False
@@ -95,6 +96,7 @@ class StarPilotState:
             self.car_state.isHKG = brand == "hyundai"
             self.car_state.isJeep = brand == "chrysler" and fallback_model_str.startswith("JEEP_")
             self.car_state.isSubaru = brand == "subaru"
+            self.car_state.isTesla = brand == "tesla"
             self.car_state.isToyota = brand == "toyota"
             self.car_state.isHKGCanFd = False
             self.car_state.hasModeStarButtons = False
@@ -170,6 +172,7 @@ class StarPilotState:
             self.car_state.isHKGCanFd = self.car_state.isHKG and safety_model == car.CarParams.SafetyModel.hyundaiCanfd
             self.car_state.isJeep = car_make == "chrysler" and car_fingerprint.startswith("JEEP_")
             self.car_state.isSubaru = car_make == "subaru"
+            self.car_state.isTesla = car_make == "tesla"
             self.car_state.isToyota = car_make == "toyota"
             self.car_state.isTSK = bool(self._safe_get(CP, "secOcRequired", False))
             self.car_state.isVolt = car_fingerprint.startswith("CHEVROLET_VOLT")

starpilot/common/assets/device_settings_layout.json

@@ -150,6 +150,17 @@
         "parent_key": "AlwaysOnLateral",
         "settings_tier": "simple"
       },
+      {
+        "key": "TeslaAOLDisengageOnBrake",
+        "label": "Disengage AOL on Brake",
+        "description": "Pressing the brake fully turns off Always On Lateral. Steering stays off after the brake is released until openpilot is engaged again or AOL is manually toggled back on.",
+        "picker_description": "Keep steering off after pressing the brake until you deliberately re-engage it.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "AlwaysOnLateral",
+        "vehicle_makes": ["Tesla"],
+        "settings_tier": "simple"
+      },
       {
         "key": "LaneChanges",
         "label": "Lane Changes",

starpilot/common/starpilot_variables.py

@@ -862,6 +862,9 @@ class StarPilotVariables:
     )
     toggle.always_on_lateral_main = toggle.always_on_lateral and not prohibited_main_aol
     toggle.always_on_lateral_pause_speed = self.get_value("PauseAOLOnBrake", cast=float, condition=toggle.always_on_lateral)
+    toggle.tesla_aol_disengage_on_brake = self.get_value(
+      "TeslaAOLDisengageOnBrake", condition=toggle.always_on_lateral and toggle.car_make == "tesla"
+    )
 
     main_cruise_button_control = self.get_button_function("MainCruiseButtonControl")
     toggle.main_cruise_aol_toggle = _main_cruise_aol_allowed(main_cruise_button_control)

starpilot/controls/starpilot_card.py

@@ -73,7 +73,9 @@ class StarPilotCard:
     self.g70_main_cruise_aol_pending_frames = 0
     self.prev_cruise_available = None
     self.prev_active = False
+    self.prev_brake_pressed = False
     self.prev_cruise_enabled = False
+    self.tesla_aol_brake_disengaged = False
     self.decel_pressed = False
     self.cancelPressed_previously = False
     self.cancel_pulse_glide_suppressed = False
@@ -161,9 +163,17 @@ class StarPilotCard:
   def _toggle_controller_aol(self, carState, starpilot_toggles):
     if not self.always_on_lateral_supported or not getattr(starpilot_toggles, "always_on_lateral", False):
       return False
+    tesla_disengage_on_brake = (
+      self.CP.brand == "tesla" and
+      getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False)
+    )
+    if tesla_disengage_on_brake and not self.always_on_lateral_allowed and carState.brakePressed:
+      return False
     if self.hyundai_aol_needs_engagement:
       self.hyundai_aol_ready = True
     self.always_on_lateral_allowed = not self.always_on_lateral_allowed
+    if tesla_disengage_on_brake and self.always_on_lateral_allowed:
+      self.tesla_aol_brake_disengaged = False
     if carState.cruiseState.enabled or self.pause_lateral:
       self.pause_lateral = not self.always_on_lateral_allowed
     return True
@@ -260,6 +270,12 @@ class StarPilotCard:
       and starpilot_toggles.main_cruise_aol_toggle
     )
     forte_main_cruise_aol_managed = self.kia_forte_non_scc and starpilot_toggles.main_cruise_aol_toggle
+    tesla_disengage_on_brake = (
+      self.CP.brand == "tesla" and
+      getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False)
+    )
+    if not tesla_disengage_on_brake:
+      self.tesla_aol_brake_disengaged = False
 
     if carState.gearShifter in NON_DRIVING_GEARS or not g70_main_cruise_aol_managed:
       self.g70_main_cruise_aol_pending = False
@@ -335,12 +351,23 @@ class StarPilotCard:
 
     # On rising edge of engagement (SET press enabling lat+long), auto-enable AOL
     # so that lateral persists when braking disengages longitudinal
-    if sm["selfdriveState"].active and not self.prev_active and self.always_on_lateral_set and starpilot_toggles.always_on_lateral_lkas:
+    engagement_started = sm["selfdriveState"].active and not self.prev_active
+    if (engagement_started and self.always_on_lateral_set and
+        (starpilot_toggles.always_on_lateral_lkas or tesla_disengage_on_brake)):
       if hyundai_aol_needs_engagement:
         self.hyundai_aol_ready = True
+      self.tesla_aol_brake_disengaged = False
       self.always_on_lateral_allowed = True
 
+    if (tesla_disengage_on_brake and carState.brakePressed and not self.prev_brake_pressed and
+        self.always_on_lateral_set):
+      self.tesla_aol_brake_disengaged = True
+
+    if self.tesla_aol_brake_disengaged:
+      self.always_on_lateral_allowed = False
+
     self.prev_active = sm["selfdriveState"].active
+    self.prev_brake_pressed = carState.brakePressed
     self.prev_cruise_enabled = carState.cruiseState.enabled
     self.prev_cruise_available = carState.cruiseState.available
 

starpilot/controls/starpilot_planner.py

@@ -166,11 +166,9 @@ class StarPilotPlanner:
 
     CS = sm["carState"]
     blinker_on = CS.leftBlinker or CS.rightBlinker
-    signal_pause = blinker_on and starpilot_toggles.pause_lateral_below_signal
 
     self.lateral_check = v_ego >= starpilot_toggles.pause_lateral_below_speed
     self.lateral_check |= not blinker_on and starpilot_toggles.pause_lateral_below_signal
-    self.lateral_check |= CS.standstill and not signal_pause
     self.lateral_check &= not sm["starpilotCarState"].pauseLateral
 
     # Blinker-based lateral resume delay: after blinker turns off, delay lateral

starpilot/controls/tests/test_starpilot_card.py

@@ -70,6 +70,7 @@ def make_toggles(**overrides):
     "always_on_lateral_lkas": False,
     "always_on_lateral_main": False,
     "always_on_lateral_pause_speed": 0.0,
+    "tesla_aol_disengage_on_brake": False,
     "bookmark_via_cancel": False,
     "bookmark_via_cancel_long": False,
     "bookmark_via_cancel_very_long": False,
@@ -1172,6 +1173,118 @@ def test_hyundai_main_aol_persists_after_brake_disengage_without_manual_aol_butt
   assert ret.alwaysOnLateralEnabled is True
 
 
+def test_tesla_aol_disengages_on_brake_until_deliberate_reengagement(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(
+    SimpleNamespace(brand="tesla", carFingerprint="TESLA_MODEL_Y", pcmCruise=True),
+    SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
+  )
+  sm = make_sm()
+  toggles = make_toggles(
+    always_on_lateral=True,
+    always_on_lateral_main=True,
+    tesla_aol_disengage_on_brake=True,
+  )
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+
+  sm["selfdriveState"].active = True
+  ret = card.update(make_car_state(available=True, enabled=True), starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralEnabled is True
+
+  sm["selfdriveState"].active = False
+  ret = card.update(
+    make_car_state(available=True, brake_pressed=True), starpilot_car_state, sm, toggles,
+  )
+  assert ret.alwaysOnLateralAllowed is False
+  assert ret.alwaysOnLateralEnabled is False
+
+  ret = card.update(
+    make_car_state(available=True, gas_pressed=True), starpilot_car_state, sm, toggles,
+  )
+  assert ret.alwaysOnLateralAllowed is False
+  assert ret.alwaysOnLateralEnabled is False
+
+  sm["selfdriveState"].active = True
+  ret = card.update(make_car_state(available=True, enabled=True), starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralAllowed is True
+  assert ret.alwaysOnLateralEnabled is True
+
+
+def test_tesla_aol_can_be_manually_reenabled_after_brake_release(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(
+    SimpleNamespace(brand="tesla", carFingerprint="TESLA_MODEL_3", pcmCruise=True),
+    SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
+  )
+  sm = make_sm()
+  toggles = make_toggles(
+    always_on_lateral=True,
+    always_on_lateral_main=True,
+    tesla_aol_disengage_on_brake=True,
+  )
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+
+  card.update(make_car_state(available=True), starpilot_car_state, sm, toggles)
+  card.update(make_car_state(available=True, brake_pressed=True), starpilot_car_state, sm, toggles)
+  released_state = make_car_state(available=True)
+  card.update(released_state, starpilot_car_state, sm, toggles)
+
+  assert card._toggle_controller_aol(released_state, toggles) is True
+  ret = card.update(released_state, starpilot_car_state, sm, toggles)
+  assert ret.alwaysOnLateralAllowed is True
+  assert ret.alwaysOnLateralEnabled is True
+
+
+def test_tesla_aol_cannot_be_reenabled_while_brake_is_held(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(
+    SimpleNamespace(brand="tesla", carFingerprint="TESLA_MODEL_Y", pcmCruise=True),
+    SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
+  )
+  toggles = make_toggles(
+    always_on_lateral=True,
+    always_on_lateral_main=True,
+    tesla_aol_disengage_on_brake=True,
+  )
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+  brake_state = make_car_state(available=True, brake_pressed=True)
+
+  card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles)
+  card.update(brake_state, starpilot_car_state, make_sm(), toggles)
+
+  assert card._toggle_controller_aol(brake_state, toggles) is False
+  assert card.always_on_lateral_allowed is False
+
+
+def test_tesla_brake_disengage_toggle_does_not_change_other_brands(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(
+    SimpleNamespace(brand="gm"),
+    SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL),
+  )
+  toggles = make_toggles(
+    always_on_lateral=True,
+    always_on_lateral_main=True,
+    tesla_aol_disengage_on_brake=True,
+  )
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+
+  card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles)
+  card.update(make_car_state(available=True, brake_pressed=True), starpilot_car_state, make_sm(), toggles)
+  ret = card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles)
+
+  assert ret.alwaysOnLateralAllowed is True
+  assert ret.alwaysOnLateralEnabled is True
+
+
 def test_aol_persists_through_longitudinal_speed_too_low_disable(monkeypatch, tmp_path):
   monkeypatch.setattr(spc, "Params", FakeParams)
   monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)

starpilot/system/the_galaxy/tests/test_device_settings_layout.py

@@ -80,6 +80,15 @@ def test_galaxy_layout_contains_basic_mode_controls():
   assert {"AlphaLongitudinalEnabled", "ForceOffroad", "GalaxyDeveloperMode"} <= sections["Developer"].keys()
 
 
+def test_tesla_aol_brake_disengage_is_tesla_only_and_opt_in():
+  setting = _params_by_section(_layout())["Lateral (Steering)"]["TeslaAOLDisengageOnBrake"]
+
+  assert _declared_default("TeslaAOLDisengageOnBrake") == "0"
+  assert setting["vehicle_makes"] == ["Tesla"]
+  assert setting["parent_key"] == "AlwaysOnLateral"
+  assert setting["ui_type"] == "toggle"
+
+
 def test_galaxy_new_ui_is_the_visible_default_choice():
   galaxy_default = _params_by_section(_layout())["Developer"]["GalaxyMobileDefault"]