From 4e27abda7d549566b8662b47c2637ff302520441 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 19 Oct 2026 23:38:06 -0500 Subject: [PATCH] Restore Toyota PCM speed overrides and standstill steering --- opendbc_repo/opendbc/car/toyota/carstate.py | 7 +- opendbc_repo/opendbc/car/toyota/interface.py | 3 + openpilot/selfdrive/car/card.py | 2 +- openpilot/selfdrive/car/cruise.py | 39 ++++- .../lateral/tests/test_lane_runtime.py | 18 +++ openpilot/starpilot/speed_limits/runtime.py | 5 +- .../speed_limits/tests/test_runtime_replay.py | 1 + .../tests/test_toyota_pcm_cruise.py | 152 ++++++++++++++++++ 8 files changed, 223 insertions(+), 4 deletions(-) create mode 100644 openpilot/starpilot/speed_limits/tests/test_toyota_pcm_cruise.py diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 5132a7c125..a0c62b62d4 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 048927d202..5b16dc0c79 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/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: diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 8a69802269..068868e7fa 100755 --- a/openpilot/selfdrive/car/card.py +++ b/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: diff --git a/openpilot/selfdrive/car/cruise.py b/openpilot/selfdrive/car/cruise.py index a8fd9d42c1..83f27d0ce0 100644 --- a/openpilot/selfdrive/car/cruise.py +++ b/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 diff --git a/openpilot/starpilot/lateral/tests/test_lane_runtime.py b/openpilot/starpilot/lateral/tests/test_lane_runtime.py index e875016fda..9ddec649b7 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_runtime.py +++ b/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) diff --git a/openpilot/starpilot/speed_limits/runtime.py b/openpilot/starpilot/speed_limits/runtime.py index ff9c3cf2e7..c0a3fa44b5 100644 --- a/openpilot/starpilot/speed_limits/runtime.py +++ b/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 diff --git a/openpilot/starpilot/speed_limits/tests/test_runtime_replay.py b/openpilot/starpilot/speed_limits/tests/test_runtime_replay.py index a7d7920fc5..a90a7b60a7 100644 --- a/openpilot/starpilot/speed_limits/tests/test_runtime_replay.py +++ b/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 diff --git a/openpilot/starpilot/speed_limits/tests/test_toyota_pcm_cruise.py b/openpilot/starpilot/speed_limits/tests/test_toyota_pcm_cruise.py new file mode 100644 index 0000000000..17bc40dd65 --- /dev/null +++ b/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)