Restore Toyota stop-and-go assistance setting

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -85,6 +85,7 @@ class CarController(CarControllerBase):
     self.last_angle = 0
     self.alert_active = False
     self.standstill_req = False
+    self.sng_hack = False
     self.last_standstill = False
     self.prius_longitudinal = prius_longitudinal.enabled(CP)
     self.sdsu_hybrid = (eps8_sdsu.configured(CP) or legacy_sdsu.configured(CP)) and bool(CP.flags & ToyotaFlags.HYBRID)
@@ -360,6 +361,9 @@ class CarController(CarControllerBase):
         if not should_resume and CS.out.cruiseState.standstill:
           self.standstill_req = True
 
+      if self.sng_hack and long_active:
+        self.standstill_req = False
+
       if (bypass_long or rav4h_long or pedal_owner or advert_long or original_prius_long) and (not filter_allowed or not long_active and CS.out.standstill):
         self.standstill_req = True
 

opendbc_repo/opendbc/car/toyota/sng.py

@@ -0,0 +1,14 @@
+"""StarPilot stop-latch override for existing Toyota longitudinal owners."""
+from opendbc.car import structs
+from opendbc.car.toyota.values import ToyotaFlags
+
+
+def eligible(cp):
+  from opendbc.car.toyota import prius_longitudinal, rav4h_filter, advert_filter, legacy_stop_timer
+  return bool(cp is not None and cp.brand == 'toyota' and cp.openpilotLongitudinalControl and
+              not cp.autoResumeSng and not cp.deprecated.enableGasInterceptor and
+              not cp.flags & ToyotaFlags.GAS_INTERCEPTOR and
+              not cp.passive and not cp.dashcamOnly and not cp.notCar and
+              len(cp.safetyConfigs) == 1 and cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.toyota and
+              (prius_longitudinal.enabled(cp) or rav4h_filter.enabled(cp) or
+               advert_filter.configured(cp) or legacy_stop_timer.enabled(cp)))

openpilot/common/params_keys.h

@@ -21,6 +21,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"PromptVolume", {PERSISTENT, INT}},
     {"AlwaysOnLateral", {PERSISTENT, BOOL, "1"}},
     {"NostalgiaMode", {PERSISTENT, BOOL, "0"}},
+    {"SNGHack", {PERSISTENT, BOOL, "0"}},
     {"ToyotaAutoHold", {PERSISTENT, BOOL, "0"}},
     {"JeepBrakeHold", {PERSISTENT, BOOL, "0"}},
     {"ToyotaRadarMode", {PERSISTENT, INT, "0"}},

openpilot/starpilot/car/toyota/tests/test_sng.py

@@ -0,0 +1,103 @@
+"""The saved donor option reaches the real settings and controller owners."""
+from dataclasses import replace
+
+import pytest
+from opendbc.car.toyota.interface import CarInterface
+from openpilot.common.params import Params
+from openpilot.common.prefix import OpenpilotPrefix
+from openpilot.starpilot.car.toyota.tests.test_gas_override_cancel import setup, acc_frames, PROFILES
+from openpilot.starpilot.car.toyota.tests.test_prius_sdsu_filter import factory
+from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner
+from openpilot.starpilot.ui.feature_settings_state import row_change
+from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
+
+
+def eligible(cp):
+  from opendbc.car.toyota.sng import eligible as check
+  return check(cp)
+
+
+def owner_for(params, cp, parked=True):
+  return FeatureSettingsOwner(params, lambda _: parked, vehicle_params=lambda: cp,
+                              vehicle_fingerprint=lambda: cp.carFingerprint)
+
+
+def rows(owner, parked=True):
+  return owner.snapshot('vehicle', parked=parked, system_long=True, lateral_context=True, metric=False).rows
+
+
+def test_saved_sng_option_is_exposed():
+  with OpenpilotPrefix():
+    params = Params()
+    assert any(row.key == 'SNGHack' for row in rows(owner_for(params, factory()))), 'TOYOTA_SNG_OPTION_MISSING'
+
+
+def test_saved_ui_choice_binds_next_controller_without_changing_cp():
+  with OpenpilotPrefix():
+    params, cp = Params(), factory()
+    before = cp.to_dict()
+    owner = owner_for(params, cp)
+    row = next(row for row in rows(owner) if row.key == 'SNGHack')
+    assert row.available and row.value == 'Off'
+    request = row_change(row)
+    assert request is not None
+    request = replace(request, confirmation=True)
+    from openpilot.starpilot.ui.vehicle_bool import VEHICLE_BOOL_KEYS, confirmation_question
+    assert request.key in VEHICLE_BOOL_KEYS and 'Toyota stop-and-go' in confirmation_question(request)
+    assert owner.apply(request) and params.get_bool('SNGHack')
+    assert not owner.apply(request)
+    ci = CarInterface(cp)
+    prefs = VehicleStartupPreferences.read(params, enabled=True)
+    prefs.configure_controller(ci)
+    assert ci.CC.sng_hack and cp.to_dict() == before
+    params.put_bool('SNGHack', False, block=True)
+    assert ci.CC.sng_hack
+    VehicleStartupPreferences.read(params, enabled=True).configure_controller(ci)
+    assert not ci.CC.sng_hack
+
+
+@pytest.mark.parametrize('veto', ('stock', 'automatic', 'pedal', 'passive', 'dashcam', 'notcar', 'native'))
+def test_donor_eligibility_and_native_identity_are_preserved(veto):
+  cp = factory()
+  assert eligible(cp)
+  if veto == 'stock':
+    cp.openpilotLongitudinalControl = False
+  elif veto == 'pedal':
+    cp.deprecated.enableGasInterceptor = True
+  elif veto == 'native':
+    cp.safetyConfigs[0].safetyModel = 'noOutput'
+  else:
+    setattr(cp, {'automatic': 'autoResumeSng', 'passive': 'passive', 'dashcam': 'dashcamOnly', 'notcar': 'notCar'}[veto], True)
+  assert not eligible(cp)
+  with OpenpilotPrefix():
+    assert all(row.key != 'SNGHack' for row in rows(owner_for(Params(), cp)))
+
+
+def test_onroad_and_safe_mode_cannot_enable_override():
+  with OpenpilotPrefix():
+    params, cp = Params(), factory()
+    owner = owner_for(params, cp, parked=False)
+    assert not next(row for row in rows(owner, False) if row.key == 'SNGHack').available
+    params.put_bool('SNGHack', True, block=True)
+    params.put_bool('SafeMode', True, block=True)
+    ci = CarInterface(cp)
+    VehicleStartupPreferences.read(params, enabled=True).configure_controller(ci)
+    assert not ci.CC.sng_hack
+
+
+@pytest.mark.parametrize('profile', PROFILES)
+@pytest.mark.parametrize('active', (False, True))
+def test_stop_latch_override_requires_qualified_active_long(profile, active):
+  cp, controller, command, state = setup(profile)
+  controller.sng_hack = eligible(cp)
+  state.out.gasPressed = False
+  state.out.standstill = state.out.cruiseState.standstill = True
+  state.out.vEgo = state.out.vEgoRaw = 0.
+  state.pcm_acc_status = 8
+  command.longActive = active
+  command.actuators.accel = -.5
+  controller.standstill_req = True
+  frames = acc_frames(cp, controller, command, state)
+  assert frames
+  assert all(bool(frame['RELEASE_STANDSTILL']) == (active and eligible(cp)) for frame in frames)
+  assert all(not frame['CANCEL_REQ'] for frame in frames)

openpilot/starpilot/car/toyota/tests/test_sng_callers.py

@@ -0,0 +1,33 @@
+"""Saved SNG opt-in retains the physical/native stop-engagement contract."""
+import pytest
+from opendbc.car.toyota.sng import eligible
+from openpilot.common.params import Params
+from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
+from openpilot.starpilot.car.toyota.tests import test_prius_stop_engagement_callers as helpers
+from openpilot.starpilot.car.toyota.tests.test_toyota_cpp_transport import host_heartbeat_bridge as host_heartbeat_bridge
+
+source_matched_native = helpers.source_matched_native
+daemon_gc_scope = helpers.daemon_gc_scope
+
+
+@pytest.mark.parametrize('advert', (False, True))
+@pytest.mark.parametrize('stock', (False, True))
+def test_saved_sng_reaches_real_native_stop_callers(monkeypatch, host_heartbeat_bridge, advert, stock):
+  put, configure = Params.put_bool, VehicleStartupPreferences.configure_controller
+  bound = []
+
+  def saved(params, key, value, *args, **kwargs):
+    result = put(params, key, value, *args, **kwargs)
+    if key == 'ToyotaAutoHold':
+      put(params, 'SNGHack', True, block=True)
+    return result
+
+  def bind(prefs, ci):
+    configure(prefs, ci)
+    assert ci.CC.sng_hack == eligible(ci.CP)
+    bound.append(ci.CC.sng_hack)
+
+  monkeypatch.setattr(Params, 'put_bool', saved)
+  monkeypatch.setattr(VehicleStartupPreferences, 'configure_controller', bind)
+  helpers.test_prius_accepts_stop_engagement_before_brake_release(monkeypatch, host_heartbeat_bridge, advert, stock)
+  assert bound and bound[-1] == (not stock)

openpilot/starpilot/ui/feature_settings_owner.py

@@ -82,7 +82,7 @@ BOOL_DEFAULTS = {
   "SubaruStopStartOff": False,
   "SubaruSNG": False,
   "SubaruRedneckCruise": False,
-  "ReverseCruise": False, "ToyotaAutoHold": False, "JeepBrakeHold": False, "VoltSNG": False, "GMAutoHold": False, "VoltOnePedalMode": False,
+  "ReverseCruise": False, "ToyotaAutoHold": False, "SNGHack": False, "JeepBrakeHold": False, "VoltSNG": False, "GMAutoHold": False, "VoltOnePedalMode": False,
   "RemapCancelToDistance": False, "LongPitch": True, "DisableOpenpilotLongitudinal": False,
   "SpeedLimitController": False, "ShowSpeedLimits": True,
   "SLCConfirmation": False,
@@ -427,6 +427,25 @@ class FeatureSettingsOwner:
     return commit_exact(self.params, key="JeepBrakeHold", max_bytes=128, raw=b"1" if request.value == "On" else b"0",
                         expected=request.expected, authorized=authorized, temp_prefix=".jeep-hold-").verified
 
+  def _toyota_sng_capability(self):
+    from opendbc.car.toyota.sng import eligible
+    cp = self.vehicle_params()
+    if not eligible(cp):
+      return None
+    return (str(cp.carFingerprint), int(cp.flags), bool(cp.autoResumeSng),
+            tuple((str(config.safetyModel), int(config.safetyParam)) for config in cp.safetyConfigs))
+
+  def _apply_toyota_sng(self, request):
+    if not request.confirmation or request.value not in ("Off", "On") or request.dependencies:
+      return False
+    def authorized():
+      return (self.authority("parked_preferences") and self.authority("vehicle") and
+              request.capability is not None and request.capability == self._toyota_sng_capability() and
+              request.vehicle_fingerprint == self.vehicle_fingerprint() and request.expected in (None, b"0", b"1"))
+    return commit_exact(self.params, key="SNGHack", max_bytes=8, expected=request.expected,
+                        raw=b"1" if request.value == "On" else b"0", authorized=authorized,
+                        temp_prefix=".toyota-sng-").verified
+
   def _auto_hold_capability(self) -> tuple | None:
     from opendbc.car.toyota.interface import toyota_auto_hold_supported
     cp = self.vehicle_params()
@@ -820,7 +839,7 @@ class FeatureSettingsOwner:
               FeatureRow("", "Always On Lateral", "Keep steering assistance active independently of cruise control", page=FeaturePage.AOL, available=True),
               FeatureRow("", "Wheel Controls", "Assign steering-wheel buttons and cruise behavior", page=FeaturePage.WHEEL, available=True)]
       if (self._long_pitch_capability() is not None or self._bolt_disable_capability() is not None or
-          self._auto_hold_capability() is not None or self._jeep_hold_capability() is not None or
+          self._auto_hold_capability() is not None or self._toyota_sng_capability() is not None or self._jeep_hold_capability() is not None or
           self._toyota_radar_capability() is not None or self._pedal_setup_capability() is not None or
           self.subaru_smoothing.capability() is not None or
           self._subaru_avh_capability() is not None or self._subaru_startup_capability() is not None or self._subaru_sng_capability() is not None or
@@ -845,6 +864,12 @@ class FeatureSettingsOwner:
                            "Hold the brake for over one second at a complete stop with cruise main on and cruise disengaged. " +
                            "Holds until you press the gas; normal cruise stop-and-go is unchanged. Applies after the next startup.")
       rows.extend([replace(row, available=row.available and self._readable("ToyotaAutoHold"), capability=capability)] if capability is not None else [])
+      sng_capability = self._toyota_sng_capability()
+      if sng_capability is not None:
+        row = self._bool_row("SNGHack", "Stop-and-Go Hack",
+                             configurable and parked and self.authority("vehicle") and self.authority("parked_preferences"),
+                             "Automatic resume for Toyota/Lexus vehicles without factory stop-and-go. Applies after the next startup.")
+        rows.append(replace(row, available=row.available and self._readable("SNGHack"), capability=sng_capability))
       radar_capability = self._toyota_radar_capability()
       if radar_capability is not None:
         from openpilot.starpilot.car.toyota.radar_preferences import CHOICES, KEY
@@ -1380,6 +1405,8 @@ class FeatureSettingsOwner:
       return self._apply_jeep_hold(request)
     if key == "AlphaLongitudinalEnabled":
       return self._apply_alpha_long(request)
+    if key == "SNGHack":
+      return self._apply_toyota_sng(request)
     if key == "ToyotaAutoHold":
       return self._apply_auto_hold(request)
     if key == "ToyotaRadarMode":

openpilot/starpilot/ui/vehicle_bool.py

@@ -1,7 +1,7 @@
 """Confirmation text for parked vehicle preferences."""
 
 VEHICLE_BOOL_KEYS = frozenset(("AlphaLongitudinalEnabled", "SubaruAvhStartup", "SubaruStopStartOff", "SubaruSNG", "SubaruRedneckCruise",
-                              "GMPedalLongitudinal", "DisableOpenpilotLongitudinal", "LongPitch", "VoltSNG", "GMAutoHold", "VoltOnePedalMode",
+                              "GMPedalLongitudinal", "DisableOpenpilotLongitudinal", "LongPitch", "VoltSNG", "SNGHack", "GMAutoHold", "VoltOnePedalMode",
                               "TeslaAOLScreenTap", "TeslaAOLDisengageOnBrake",
                               "RemoteStartBootsComma", "HKGRemoteStartBootsComma", "IgnoreIgnitionLine", "TeslaWakeOnCAN"))
 
@@ -30,6 +30,8 @@ def confirmation_question(request):
     return f"{verb} Subaru stop-and-go assistance after the next startup? Factory ACC retains speed control."
   if request.key == "SubaruRedneckCruise":
     return f"{verb} software cruise after the next startup? Stock ACC keeps pedal control."
+  if request.key == "SNGHack":
+    return f"{verb} Toyota stop-and-go assistance after the next startup?"
   if request.key == "VoltSNG":
     return f"{verb} Volt stop-and-go assistance after the next startup?"
   if request.key == "GMAutoHold":

openpilot/starpilot/vehicle_preferences.py

@@ -20,6 +20,7 @@ def bolt_disable_supported(cp) -> bool:
 @dataclass(frozen=True)
 class VehicleStartupPreferences:
   toyota_auto_hold: bool = False
+  toyota_sng: bool = field(default=False, kw_only=True)
   jeep_brake_hold: bool = field(default=False, kw_only=True)
   turn_assist: bool = False
   gm_long_pitch: bool = True
@@ -116,7 +117,9 @@ class VehicleStartupPreferences:
                ford_lateral_mode=ford_mode(params, enabled=enabled),
                ford_pinion=bool(enabled and read_saved(params, "SafeMode", 8) in ((None, True), (b"0", True)) and
                                 read_saved(params, "FordPrefSteerAngleCurvature", 8) == (b"1", True)), tesla_screen=screen,
-               tesla_screen_brake=screen_brake, toyota_auto_hold=toyota, jeep_brake_hold=jeep_hold,
+               tesla_screen_brake=screen_brake, toyota_auto_hold=toyota,
+               toyota_sng=bool(enabled and readable and safe in (None, b"0") and
+                               read_saved(params, "SNGHack", 8) == (b"1", True)), jeep_brake_hold=jeep_hold,
                gm_longitudinal_tune=selected_tune(params, enabled=enabled),
                toyota_lock_doors=bool(enabled and readable and safe in (None, b"0") and
                                      read_saved(params, "LockDoors", 8) == (b"1", True)),
@@ -276,6 +279,8 @@ class VehicleStartupPreferences:
                                      is_volt_one_pedal, camera_acc_pedal_profile, volt_cc_pedal_profile)
     cp = ci.CP
     if ci.CC is not None and cp.brand == "toyota" and not cp.passive and not cp.dashcamOnly and not cp.notCar:
+      from opendbc.car.toyota.sng import eligible
+      ci.CC.sng_hack = self.toyota_sng and eligible(cp)
       ci.CC.door_locks.lock = self.toyota_lock_doors
       ci.CC.door_locks.unlock = self.toyota_unlock_doors
     if ci.CC is not None and is_volt_longitudinal(cp):

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "350f0562c24df598ab6d98691eb08c22e1b5f6d92813a8cc2daa6438def63df2", "schema_version": 1}
+{"protected_inventory_sha256": "e6f38ae38aa7da15e11b0a0c31df2d4622d67eebf114309e07e77717ea6b61ea", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -142,6 +142,8 @@ AOL_LIFECYCLE_TARGETS = (
   "openpilot/starpilot/car/toyota/tests/test_camera_2aa_native.py",
   "openpilot/starpilot/car/toyota/tests/test_prius_sdsu_filter.py",
   "openpilot/starpilot/car/toyota/tests/test_prius_sdsu_callers.py",
+  "openpilot/starpilot/car/toyota/tests/test_sng.py",
+  "openpilot/starpilot/car/toyota/tests/test_sng_callers.py",
   "openpilot/starpilot/car/toyota/tests/test_brake_stop_cancel.py",
   "openpilot/starpilot/car/toyota/tests/test_prius_stop_engagement_callers.py",
   "opendbc_repo/opendbc/safety/tests/test_toyota_prius_sdsu.py",