Restore Toyota PCM speed overrides and standstill steering

opendbc_repo/opendbc/car/toyota/carstate.py

@@ -173,6 +173,7 @@ class CarState(CarStateBase):
         ret.accFaulted = ret.accFaulted or cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
 
     pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
+    previous_pcm_acc_status = self.pcm_acc_status
     self.pcm_acc_status = pcm_acc_status
     ret.cruiseState.standstill = pcm_acc_status == 7
     ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
@@ -191,7 +192,11 @@ class CarState(CarStateBase):
     if not (self.CP.flags & ToyotaFlags.UNSUPPORTED_DSU):
       self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
 
-    buttonEvents = []
+    # PCM exposes the actual +/- stalk state even when it owns selected speed.
+    # Keep these edges independent of SET_SPEED: the update can arrive on release.
+    buttonEvents = create_button_events(int(pcm_acc_status) if pcm_acc_status in (9, 10) else 0,
+                                       int(previous_pcm_acc_status) if previous_pcm_acc_status in (9, 10) else 0,
+                                       {9: ButtonType.accelCruise, 10: ButtonType.decelCruise})
     if self.CP.flags & (ToyotaFlags.TSS2 | ToyotaFlags.LONG_FILTER) or self.CP.carFingerprint == CAR.TOYOTA_PRIUS_RETROFIT:
       # lkas button is wired to the camera
       prev_lkas_button = self.lkas_button

opendbc_repo/opendbc/car/toyota/interface.py

@@ -160,6 +160,9 @@ class CarInterface(CarInterfaceBase):
       ret.flags |= int(ToyotaFlags.HYBRID | ToyotaFlags.LONG_FILTER)
       ret.safetyConfigs[0].safetyParam = 4169
       ret.openpilotLongitudinalControl = True
+    # Toyota EPS accepts lateral commands down to zero. Keep the existing
+    # native/axis/fault gates; a stock cruise stop does not withdraw steering.
+    ret.steerAtStandstill = ret.minSteerSpeed <= 0.
     ret.autoResumeSng = ret.openpilotLongitudinalControl and not ret.flags & ToyotaFlags.LONG_FILTER
 
     if not ret.openpilotLongitudinalControl:

openpilot/selfdrive/car/card.py

@@ -1022,7 +1022,7 @@ class Car:
       self.pm.send('slcDashboardObservation', source)
 
     change = self.v_cruise_helper.slc_cruise_change
-    if (self.slc_replay or self.curve_replay) and not self.CP.pcmCruise:
+    if self.slc_replay or self.curve_replay:
       if change is not None:
         self.slc_receipts.append(('driverChange', change, change[0], change[1], int(cs_send.logMonoTime)))
       for kind, detail, previous_mps, selected_mps, observed_ns in self.slc_receipts:

openpilot/selfdrive/car/cruise.py

@@ -59,7 +59,9 @@ class VCruiseHelper:
     self.pending_resume_ns = 0
     self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
     self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
-    # Set only by the non-PCM button path that actually changes vCruise.
+    self.pcm_slc_sample: tuple[float, int] | None = None
+    self.pcm_slc_button = ButtonType.unknown
+    # Set by software cruise steps or a checked Toyota PCM physical +/- change.
     self.slc_cruise_change: tuple[float, float, str, bool] | None = None
     self.slc_consumed_button: SlcConsumedButton | None = None
     self.slc_suppressed_buttons: set[ButtonType] = set()
@@ -99,6 +101,7 @@ class VCruiseHelper:
       else:
         self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
         self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
+        self._update_toyota_pcm_slc_change(CS, enabled, now_ns)
         if CS.cruiseState.speed == 0:
           self.v_cruise_kph = V_CRUISE_UNSET
           self.v_cruise_cluster_kph = V_CRUISE_UNSET
@@ -108,9 +111,43 @@ class VCruiseHelper:
     else:
       self.v_cruise_kph = V_CRUISE_UNSET
       self.v_cruise_cluster_kph = V_CRUISE_UNSET
+      self.pcm_slc_sample = None
+      self.pcm_slc_button = ButtonType.unknown
       if not self.CP.pcmCruise:
         self.update_button_timers(CS, enabled)
 
+  def _update_toyota_pcm_slc_change(self, CS, enabled: bool, now_ns: int) -> None:
+    """Observe a physical PCM speed change without writing stock selected speed."""
+    previous = self.pcm_slc_sample
+    button = self.pcm_slc_button
+    self.pcm_slc_sample = None
+    self.pcm_slc_button = ButtonType.unknown
+    if (self.CP.brand != 'toyota' or not self.CP.openpilotLongitudinalControl or
+        not enabled or not CS.canValid or CS.canTimeout or not CS.cruiseState.enabled or
+        CS.standstill or CS.cruiseState.standstill or
+        not math.isfinite(self.v_cruise_kph) or not V_CRUISE_MIN <= self.v_cruise_kph <= V_CRUISE_MAX):
+      return
+    continuous = previous is not None and 0 < now_ns - previous[1] <= 150_000_000
+    if not continuous:
+      button = ButtonType.unknown
+    # A held stalk can generate multiple PCM steps. On release, the previous
+    # held direction remains eligible for this frame only; no guessed time window.
+    held = button
+    for edge in CS.buttonEvents:
+      if edge.type in (ButtonType.accelCruise, ButtonType.decelCruise):
+        if edge.pressed:
+          button = held = edge.type
+        elif edge.type == held:
+          held = ButtonType.unknown
+    if continuous:
+      delta = self.v_cruise_kph - previous[0]
+      if ((button == ButtonType.accelCruise and delta > 0.) or
+          (button == ButtonType.decelCruise and delta < 0.)):
+        self.slc_cruise_change = (previous[0] / 3.6, self.v_cruise_kph / 3.6,
+                                  'accel' if delta > 0. else 'decel', False)
+    self.pcm_slc_button = held
+    self.pcm_slc_sample = (float(self.v_cruise_kph), now_ns)
+
   def _update_slc_button_latches(self, CS, enabled, slc_pending: SlcPendingConfirmation | None) -> None:
     for b in CS.buttonEvents:
       button = b.type.raw

openpilot/starpilot/lateral/tests/test_lane_runtime.py

@@ -265,6 +265,24 @@ class LaneRuntimeTests(unittest.TestCase):
           self.assertEqual(params.get('PauseLateralSpeed'), 0.)
           params.put('PauseLateralSpeed', 10., block=True)
 
+  def test_toyota_aol_remains_active_at_standstill_with_current_native_permission(self):
+    with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1'}), \
+         mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
+      params = Params()
+      cp = publish_corolla_params(params)
+      params.put('CarParams', cp.to_bytes(), block=True)
+      controls = Controls()
+      self.assertEqual(cp.minSteerSpeed, 0.)
+      for tick, (speed, allowed, fault) in enumerate(((2., True, False), (0., True, False),
+                                                     (0., False, False), (0., True, True),
+                                                     (0., True, False), (2., True, False))):
+        feed(controls, 2_000_000_000 + tick * 10_000_000, tick, speed=speed,
+             active=False, enabled=False, independent=True, native_allowed=allowed, fault=fault)
+        command, _ = controls.state_control()
+        self.assertEqual(command.latActive, allowed and not fault, (tick, speed))
+        self.assertFalse(command.longActive)
+        self.assertFalse(command.enabled)
+
   def test_strength_saved_values_and_live_refresh_keep_existing_admission(self):
     with tempfile.TemporaryDirectory() as temporary:
       root = Path(temporary)

openpilot/starpilot/speed_limits/runtime.py

@@ -222,8 +222,11 @@ class Runtime:
     self.ledger = resolved.state
     if resolved.errors:
       return None
+    # Card serializes the event's m/s as Float32, while carState carries kph.
+    # The guard above already binds them within transport precision. Classify
+    # in the exact current selected-speed coordinate used by the reducer.
     classified = arb.step(self.ledger, arb.CruiseChange(self.session_id, self.ledger.last_effect_id + 1, action_id,
-                                                         event.previous_mps, event.selected_mps), now_ns=now_ns)
+                                                         event.previous_mps, float(sm['carState'].vCruise) / 3.6), now_ns=now_ns)
     self.ledger = classified.state
     completed = arb.step(self.ledger, arb.CompleteAction(self.session_id, action_id), now_ns=now_ns)
     self.ledger = completed.state

openpilot/starpilot/speed_limits/tests/test_runtime_replay.py

@@ -634,6 +634,7 @@ class RuntimeReplayTests(unittest.TestCase):
     fake.CC_prev.enabled = False
     fake.CS_prev = car.CarState(vEgo=20.0)
     fake.experimental_mode = False
+    fake.initial_cruise_speed = 40
     fake.slc_last_command_id = 0
     fake.slc_command_session = ''
     fake.v_cruise_helper.v_cruise_kph = 60.0

openpilot/starpilot/speed_limits/tests/test_toyota_pcm_cruise.py

@@ -0,0 +1,152 @@
+"""Toyota physical PCM +/- edges must reach the SLC intent ledger."""
+from unittest import mock
+from types import SimpleNamespace
+from collections import deque
+
+import pytest
+
+from opendbc.can import CANPacker
+from opendbc.car import Bus, structs
+from opendbc.car.toyota.interface import CarInterface
+from opendbc.car.toyota.values import CAR, DBC
+from openpilot.selfdrive.car.cruise import VCruiseHelper
+from openpilot.selfdrive.car.card import Car
+from openpilot.cereal import messaging
+from openpilot.selfdrive.controls.plannerd import queue_cruise_event, current_cruise_event
+from openpilot.starpilot.speed_limits.runtime import Runtime
+from openpilot.starpilot.speed_limits.runtime_settings import parse
+from openpilot.starpilot.speed_limits.tests import test_runtime_replay as replay_fixture
+
+
+def params():
+  rack = structs.CarParams.CarFw.new_message(ecu=structs.CarParams.Ecu.eps,
+                                             fwVersion=b'8965B47070' + bytes(6))
+  return CarInterface.get_params(CAR.TOYOTA_PRIUS, {0: {0x2FF: 8}, 1: {}, 2: {}}, [rack], False, False, False)
+
+
+class ParsedCruise:
+  def __init__(self):
+    self.cp = params()
+    self.ci = CarInterface(self.cp)
+    self.packer = CANPacker(DBC[self.cp.carFingerprint][Bus.pt])
+    self.helper = VCruiseHelper(self.cp)
+    self.now = 2_000_000_000
+
+  def step(self, speed, status=8, *, active=True, enabled=True, standstill=False, healthy=True):
+    self.now += 10_000_000
+    # Synthetic CAN acquisition, actual parser/state/button decoding. The CI
+    # checks all lazy subscriptions: warm independent inputs, never set CS.
+    frames = [self.packer.make_can_msg(name, 0, {}) for name in
+              ('VSC1S07', 'BODY_CONTROL_STATE', 'BODY_CONTROL_STATE_2', 'ESP_CONTROL',
+               'GEAR_PACKET', 'PCM_CRUISE_SM', 'BLINKERS_STATE', 'PRE_COLLISION', 'LIGHT_STALK', 'BRAKE_MODULE')]
+    frames += [self.packer.make_can_msg('WHEEL_SPEEDS', 0, {f'WHEEL_SPEED_{w}': 0. if standstill else 72. for w in ('FL', 'FR', 'RL', 'RR')}),
+               self.packer.make_can_msg('PCM_CRUISE', 0, {'CRUISE_ACTIVE': int(active), 'GAS_RELEASED': 1, 'CRUISE_STATE': status}),
+               self.packer.make_can_msg('PCM_CRUISE_2', 0, {'SET_SPEED': speed, 'MAIN_ON': 1}),
+               self.packer.make_can_msg('STEER_ANGLE_SENSOR', 0, {}),
+               self.packer.make_can_msg('STEER_TORQUE_SENSOR', 0, {}),
+               self.packer.make_can_msg('EPS_STATUS', 0, {'LKA_STATE': 1}),
+               self.packer.make_can_msg('SDSU', 0, {'STATE': 0}),
+               self.packer.make_can_msg('LKAS_HUD', 2, {}), self.packer.make_can_msg('RSA1', 2, {})]
+    state = self.ci.update([(self.now, frames if healthy else [])])
+    with mock.patch('openpilot.selfdrive.car.cruise.monotonic_ns', return_value=self.now):
+      self.helper.update_v_cruise(state, enabled, True)
+    return state, self.helper.slc_cruise_change
+
+  def warm(self):
+    for _ in range(12):
+      self.step(90)
+
+
+def publish_change(parsed, state):
+  """Actual Card publication with transport/radar collaborators replaced."""
+  host = Car.__new__(Car)
+  host.CP = parsed.cp
+  host.car_params_published = True
+  host.sm = SimpleNamespace(frame=1, all_checks=lambda services: True)
+  host.last_actuators_output = structs.CarControl.Actuators()
+  host.rk = SimpleNamespace(remaining=0.)
+  host.can_rcv_cum_timeout_counter = 0
+  host.aol_replay = host.curve_replay = False
+  host.slc_replay = True
+  host.slc_producer_session = 'pcm-card'
+  host.slc_cruise_event_id = 0
+  host.v_cruise_helper = parsed.helper
+  host.slc_receipts = []
+  host.pm = mock.Mock()
+  host.car_gps_publisher = mock.Mock()
+  host.CI = SimpleNamespace(CS=SimpleNamespace())
+  host.radar_publish = mock.Mock()
+  # This is the same selected-speed copy normally made by Card.state_update.
+  state.vCruise = parsed.helper.v_cruise_kph
+  state.vCruiseCluster = parsed.helper.v_cruise_cluster_kph
+  with mock.patch('openpilot.selfdrive.car.card.REPLAY', True):
+    host.can_log_mono_time = parsed.now
+    host.state_publish(state, None)
+  sent = {call.args[0]: call.args[1] for call in host.pm.send.call_args_list}
+  assert 'slcCruiseEvent' in sent
+  # Match the actual wire, not an in-memory double-precision CruiseEvent.
+  wire = messaging.log_from_bytes(sent['slcCruiseEvent'].to_bytes())
+  queued, curve = deque(), deque()
+  queue_cruise_event(wire, queued, curve)
+  return current_cruise_event(queued, parsed.now), sent['carState']
+
+
+def test_pcm_resume_edge_and_release_frame_set_speed_feed_override():
+  parsed = ParsedCruise()
+  parsed.warm()
+  state, change = parsed.step(90, 9)
+  assert state.canValid and state.buttonEvents[0].type == structs.CarState.ButtonEvent.Type.accelCruise
+  assert change is None
+  state, change = parsed.step(92, 8)  # Real route: update can coincide with release.
+  assert state.buttonEvents[0].pressed is False
+  assert change[:2] == pytest.approx((25., 92. / 3.6))
+  assert change[2:] == ('accel', False)
+
+  # The existing runtime receives the same classified Card event; stock selected
+  # speed still belongs to PCM and the SLC ceiling follows qualified driver intent.
+  fixture = replay_fixture.RuntimeReplayTests()
+  fixture.setUp()
+  sm = fixture.sm
+  sm['carState'].vCruise = sm['carState'].vCruiseCluster = 90.
+  runtime = Runtime(parse({'SpeedLimitController': True, 'SLCPriority1': 'Dashboard'}), session_id='pcm-drive')
+  first = runtime.step(sm, parsed.cp, now_ns=2_000_000_000)
+  assert first.result.override.context is not None
+  receipt, published = publish_change(parsed, state)
+  sm.advance(parsed.now)
+  sm.data['carState'] = published.carState
+  result = runtime.step(sm, parsed.cp, now_ns=parsed.now,
+                        cruise_event=receipt)
+  assert result.result.override.event_receipt.consumed
+  assert result.result.override.state.persistent_selected_mps == pytest.approx(change[1])
+  assert parsed.helper.v_cruise_kph == pytest.approx(92.)
+
+
+@pytest.mark.parametrize('mode', ('no_button', 'wrong_direction', 'disengage', 'stop_resume', 'invalid', 'gap', 'reengage'))
+def test_pcm_speed_changes_without_current_matching_driver_edge_do_not_arm(mode):
+  parsed = ParsedCruise()
+  parsed.warm()
+  if mode == 'gap':
+    parsed.step(90, 9)
+    parsed.now += 1_000_000_000
+  elif mode == 'reengage':
+    parsed.step(90, 0, active=False, enabled=False)
+  elif mode == 'invalid':
+    for _ in range(60):
+      parsed.step(90, 9, healthy=False)
+  status = 10 if mode == 'wrong_direction' else 9 if mode in ('stop_resume', 'disengage', 'reengage') else 8
+  _, change = parsed.step(92, status, active=mode != 'disengage', enabled=mode != 'disengage', standstill=mode == 'stop_resume')
+  assert change is None
+
+
+def test_pcm_held_plus_and_minus_keep_selected_owner_and_do_not_duplicate():
+  parsed = ParsedCruise()
+  parsed.warm()
+  _, change = parsed.step(92, 9)
+  assert change[:2] == pytest.approx((25., 92. / 3.6))
+  assert change[2:] == ('accel', False)
+  assert parsed.step(92, 9)[1] is None
+  assert parsed.step(94, 9)[1][:2] == pytest.approx((92. / 3.6, 94. / 3.6))
+  parsed.step(94, 8)
+  change = parsed.step(92, 10)[1]
+  assert change[:2] == pytest.approx((94. / 3.6, 92. / 3.6))
+  assert change[2:] == ('decel', False)