diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index bcff7f7c46..d8cb458735 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/toyota/sng.py b/opendbc_repo/opendbc/car/toyota/sng.py new file mode 100644 index 0000000000..15adc42896 --- /dev/null +++ b/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))) diff --git a/openpilot/common/libcommon.a b/openpilot/common/libcommon.a index aeb269d302..9c5b705afe 100644 Binary files a/openpilot/common/libcommon.a and b/openpilot/common/libcommon.a differ diff --git a/openpilot/common/libparams_c.so b/openpilot/common/libparams_c.so index 9330bcea64..b4a0b208ff 100755 Binary files a/openpilot/common/libparams_c.so and b/openpilot/common/libparams_c.so differ diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 9d4399af8f..2a5fd08e9c 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -21,6 +21,7 @@ inline static std::unordered_map 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"}}, diff --git a/openpilot/starpilot/car/toyota/tests/test_sng.py b/openpilot/starpilot/car/toyota/tests/test_sng.py new file mode 100644 index 0000000000..3c58a10544 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/toyota/tests/test_sng_callers.py b/openpilot/starpilot/car/toyota/tests/test_sng_callers.py new file mode 100644 index 0000000000..a1878a6bdb --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/ui/feature_settings_owner.py b/openpilot/starpilot/ui/feature_settings_owner.py index 32fe8738ae..e01ffe067d 100644 --- a/openpilot/starpilot/ui/feature_settings_owner.py +++ b/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": diff --git a/openpilot/starpilot/ui/vehicle_bool.py b/openpilot/starpilot/ui/vehicle_bool.py index 764b2bb676..f698153528 100644 --- a/openpilot/starpilot/ui/vehicle_bool.py +++ b/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": diff --git a/openpilot/starpilot/vehicle_preferences.py b/openpilot/starpilot/vehicle_preferences.py index 7d1c44a86a..332747aa04 100644 --- a/openpilot/starpilot/vehicle_preferences.py +++ b/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): diff --git a/prebuilt.json b/prebuilt.json index 15586d69e4..39ebd46a0a 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "350f0562c24df598ab6d98691eb08c22e1b5f6d92813a8cc2daa6438def63df2", "schema_version": 1} +{"protected_inventory_sha256": "e6f38ae38aa7da15e11b0a0c31df2d4622d67eebf114309e07e77717ea6b61ea", "schema_version": 1} diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index e53be92797..e6c76005e7 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/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",