Fix Prius stop-and-go settings and stopped resume

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

@@ -0,0 +1,95 @@
+"""Release while PCM and wheel speed still report stopped; no RES injection."""
+import pytest
+from opendbc.car import structs
+from opendbc.car.toyota import prius_advert_filter, prius_sdsu, toyotacan
+from opendbc.car.toyota.values import CAR
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
+from openpilot.starpilot.car.toyota.tests.test_prius_sdsu_filter import factory, original_firmware
+from openpilot.starpilot.car.toyota.tests import test_rav4h_caller_native 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
+
+
+def stopped():
+  cs = structs.CarState(canValid=True, standstill=True)
+  cs.cruiseState.enabled = cs.cruiseState.available = cs.cruiseState.standstill = True
+  return cs
+
+
+def test_sng_planned_release_without_pcm_or_wheel_motion():
+  control = LongControl(factory(), VehicleStartupPreferences(toyota_sng=True))
+  cs = stopped()
+  for _ in range(20):
+    control.update(True, cs, -.5, True, (-3., 2.))
+  outputs = [control.update(True, cs, .8, False, (-3., 2.)) for _ in range(100)]
+  assert max(outputs) > 0, 'PRIUS_SNG_PCM_STANDSTILL_DEADLOCK'
+  assert cs.standstill and cs.cruiseState.standstill and not cs.buttonEvents
+
+
+@pytest.mark.parametrize('sng,active,brake,should_stop', ((False, True, False, False),
+                          (True, False, False, False), (True, True, True, False), (True, True, False, True)))
+def test_sng_does_not_release_default_off_inactive_brake_or_force_stop(sng, active, brake, should_stop):
+  control = LongControl(factory(), VehicleStartupPreferences(toyota_sng=sng))
+  cs = stopped()
+  cs.brakePressed = brake
+  for _ in range(100):
+    assert control.update(active, cs, .8, should_stop, (-3., 2.)) <= 0.
+
+
+@pytest.mark.parametrize('advert', (False, True))
+def test_actual_native_drive_request_precedes_wheel_motion(monkeypatch, host_heartbeat_bridge, advert):
+  tick, positive = 0, False
+  was_stopped = True
+  physical, inputs, put = helpers.physical, helpers.control_inputs, Params.put_bool
+  accel_command = toyotacan.create_accel_command
+
+  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 frames(packer, **fields):
+    nonlocal tick, was_stopped
+    tick += 1
+    was_stopped = not positive
+    fields.update(stopped=was_stopped, brake=tick < 200)
+    return physical(packer, **fields)
+
+  def plans(*args, **kwargs):
+    peers = inputs(*args, **kwargs)
+    for index, peer in enumerate(peers):
+      if peer.which() == 'longitudinalPlan':
+        plan = peer.as_builder() if hasattr(peer, 'as_builder') else peer
+        plan.longitudinalPlan.shouldStop = tick < 220
+        plan.longitudinalPlan.aTarget = -.5 if tick < 220 else .8
+        peers[index] = plan
+    return peers
+
+  def observe(packer, accel, cancel, *args, **kwargs):
+    nonlocal positive
+    if accel > 0 and not positive:
+      assert was_stopped and tick >= 220 and not cancel
+      positive = True
+    return accel_command(packer, accel, cancel, *args, **kwargs)
+
+  def inspect(tick, host, cs, cc, safety, native):
+    assert host.CI.CC.sng_hack
+    if 180 <= tick < 190:
+      assert cs.brakePressed and not cc.longActive and safety.aol_get_permission_mask() & 2 == 0
+    if 210 <= tick < 219:
+      assert cs.standstill and cs.cruiseState.standstill and cc.actuators.accel <= 0 and not positive
+
+  monkeypatch.setattr(Params, 'put_bool', saved)
+  monkeypatch.setattr(helpers, 'physical', frames)
+  monkeypatch.setattr(helpers, 'control_inputs', plans)
+  monkeypatch.setattr(toyotacan, 'create_accel_command', observe)
+  owner = prius_advert_filter if advert else prius_sdsu
+  helpers.run_case(False, candidate=CAR.TOYOTA_PRIUS, bridge=host_heartbeat_bridge, input_clock=True,
+                   firmware_override=original_firmware(), filter_advert=advert,
+                   sdsu_words=(owner.LONG_WORD, owner.STOCK_WORD), on_sample=inspect)
+  assert positive

openpilot/starpilot/galaxy/tests/test_prius_sng_settings.py

@@ -0,0 +1,80 @@
+"""Real settings authority for original-rack Prius SDSU and manual selection."""
+import json
+from dataclasses import replace
+from pathlib import Path
+from types import SimpleNamespace
+from unittest.mock import Mock
+
+import pytest
+from opendbc.car.toyota.values import CAR
+from openpilot.common.params import Params
+from openpilot.starpilot.car.toyota.tests.test_prius_sdsu_filter import factory
+from openpilot.starpilot.galaxy.settings import AuthorityContext, SettingsGateway, SettingsChanged, _qualified
+from openpilot.starpilot.galaxy.vehicle_configuration import settings_configuration
+from openpilot.starpilot.vehicle_selection import choices, encode
+
+
+def gateway(params, cp, raw):
+  current = SimpleNamespace(parked=True, cp=cp, raw=raw)
+  source = SimpleNamespace(sample=lambda: AuthorityContext(current.parked, current.cp, current.raw))
+  return current, SettingsGateway(params, Mock(wraps=source), clock=lambda: 10)
+
+
+@pytest.mark.parametrize('manual', (False, True))
+def test_real_gateway_sng_saves_without_auto_hold_authority(tmp_path, manual):
+  params, physical = Params(str(tmp_path)), factory()
+  params.put('VehicleSelection', json.loads(encode(str(CAR.TOYOTA_PRIUS) if manual else None)), block=True)
+  cp, raw, _, _ = settings_configuration(params, physical, b'physical-prius')
+  assert cp is not None and cp.safetyConfigs[0].safetyParam == physical.safetyConfigs[0].safetyParam
+  assert cp.openpilotLongitudinalControl and not cp.alphaLongitudinalAvailable
+  current, owner = gateway(params, cp, raw)
+  assert not _qualified(AuthorityContext(True, cp, raw), 'vehicle')
+  for value in ('On', 'Off'):
+    page = owner.page('vehicle', 'session', b'generation')
+    index = next(i for i, row in enumerate(page['rows']) if row['label'] == 'Stop-and-Go Hack')
+    assert page['rows'][index]['available'], 'PRIUS_SNG_WRONG_AUTO_HOLD_GATE'
+    intent = owner.preview(page['view'], index, 0, 'session', b'generation', value=value)
+    assert owner.confirm(intent['intent'], 'session', b'generation')
+    assert Path(params.get_param_path('SNGHack')).read_bytes() == (b'1' if value == 'On' else b'0')
+
+
+def test_manual_prius_labels_distinguish_original_rack_and_retrofit():
+  labels = {choice.platform: choice.label for choice in choices()}
+  assert 'Prius Prime' in labels[CAR.TOYOTA_PRIUS], 'PRIUS_MANUAL_LABEL_AMBIGUOUS'
+  assert '2016-20' in labels[CAR.TOYOTA_PRIUS]
+  assert 'TSS2 EPS retrofit' in labels[CAR.TOYOTA_PRIUS_RETROFIT]
+
+
+@pytest.mark.parametrize('change', ('onroad', 'profile', 'bytes', 'session'))
+def test_pending_sng_choice_cannot_survive_authority_or_source_change(tmp_path, change):
+  params, cp = Params(str(tmp_path)), factory()
+  current, owner = gateway(params, cp, b'physical-prius')
+  page = owner.page('vehicle', 'session', b'generation')
+  index = next(i for i, row in enumerate(page['rows']) if row['label'] == 'Stop-and-Go Hack')
+  intent = owner.preview(page['view'], index, 0, 'session', b'generation', value='On')
+  if change == 'onroad':
+    current.parked = False
+  elif change == 'profile':
+    current.cp = None
+    current.raw = b'changed'
+  elif change == 'bytes':
+    params.put_bool('SNGHack', False, block=True)
+  try:
+    assert not owner.confirm(intent['intent'], 'session', b'generation', session_valid=lambda: change != 'session')
+  except SettingsChanged:
+    assert change != 'bytes'
+  assert not params.get_bool('SNGHack')
+
+
+def test_native_owner_sng_save_does_not_require_unrelated_auto_hold(tmp_path):
+  from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner
+  from openpilot.starpilot.ui.feature_settings_state import row_change
+  params, cp = Params(str(tmp_path)), factory()
+  owner = FeatureSettingsOwner(params, lambda group: group == 'parked_preferences',
+                               vehicle_params=lambda: cp, vehicle_fingerprint=lambda: cp.carFingerprint)
+  row = next(row for row in owner.snapshot('vehicle', parked=True, system_long=True,
+                                          lateral_context=True, metric=False).rows if row.key == 'SNGHack')
+  request = row_change(row)
+  assert request is not None
+  assert owner.apply(replace(request, confirmation=True))
+  assert params.get_bool('SNGHack')

openpilot/starpilot/longitudinal/extension.py

@@ -43,7 +43,9 @@ class LongitudinalExtension:
     self.vehicle_target = getattr(self.vehicle_policy, "target", None)
     self.kp = self.vehicle_policy.kp if self.vehicle_policy is not None else 0.0
     self.ki = getattr(self.vehicle_policy, "ki", None)
-    self.ignore_cruise_standstill = bool(getattr(self.vehicle_policy, "ignore_cruise_standstill", False))
+    from opendbc.car.toyota.sng import eligible as toyota_sng_eligible
+    self.ignore_cruise_standstill = bool(getattr(self.vehicle_policy, "ignore_cruise_standstill", False) or
+      getattr(startup_preferences, "toyota_sng", False) and toyota_sng_eligible(CP))
 
   def qualify_resume(self, default, *, preferences, inputs):
     if self.resume_policy is None or not preferences.volt_sng:
@@ -157,6 +159,7 @@ class LongitudinalExtension:
 def create_extension(cp, startup_preferences=None):
   extension = LongitudinalExtension(cp, startup_preferences)
   if (extension.vehicle_policy is None and extension.toyota_output is None and extension.ioniq6_start is None and
-      extension.bolt_mode is None and extension.stopping_policy is None and extension.stopping_decel_rate == 1.0):
+      extension.bolt_mode is None and extension.stopping_policy is None and extension.stopping_decel_rate == 1.0 and
+      not extension.ignore_cruise_standstill):
     return None
   return extension

openpilot/starpilot/ui/feature_settings_owner.py

@@ -439,7 +439,7 @@ class FeatureSettingsOwner:
     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
+      return (self.authority("parked_preferences") 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,
@@ -867,7 +867,7 @@ class FeatureSettingsOwner:
       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"),
+                             configurable and parked 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()

openpilot/starpilot/vehicle_selection.py

@@ -38,6 +38,10 @@ def choices() -> tuple[VehicleChoice, ...]:
     docs = member.config.car_docs
     make = docs[0].make if docs else member.__class__.__module__.split('.')[-2].title()
     label = docs[0].name if len(docs) == 1 else platform.replace('_', ' ').title()
+    if platform == "TOYOTA_PRIUS":
+      label = "Toyota Prius / Prius Prime 2016-20"
+    elif platform == "TOYOTA_PRIUS_RETROFIT":
+      label = "Toyota Prius 2016-20 (TSS2 EPS retrofit)"
     result.append(VehicleChoice(platform, make, label))
   return tuple(sorted(result, key=lambda item: (item.make, item.label, item.platform)))
 

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "fae579a36e85bd4de2f49ef61f39c89c00a3f8392fe1ede3b8c791611fa7133e", "schema_version": 1}
+{"protected_inventory_sha256": "5ee191de2be6e6c25a0d884b343a5a82a93e70d2fea1b8189d4733d54f7a1706", "schema_version": 1}

tools/ci/run_vehicle_tests.py

@@ -148,6 +148,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/galaxy/tests/test_prius_sng_settings.py",
+  "openpilot/starpilot/car/toyota/tests/test_sng_resume.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",