From f5c56bc63140d3aede92b15477a05bb32b68caf4 Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Tue, 20 Oct 2026 17:56:01 -0500 Subject: [PATCH] ZSS support Co-Authored-By: Erich Moraga <33645296+erichmoraga@users.noreply.github.com> --- .../opendbc/car/toyota/carcontroller.py | 1 + opendbc_repo/opendbc/car/toyota/carstate.py | 26 ++++++ opendbc_repo/opendbc/car/toyota/interface.py | 2 + .../opendbc/car/toyota/prius_longitudinal.py | 2 +- .../opendbc/car/toyota/tests/test_zss.py | 81 +++++++++++++++++++ opendbc_repo/opendbc/car/toyota/values.py | 1 + 6 files changed, 112 insertions(+), 1 deletion(-) create mode 100644 opendbc_repo/opendbc/car/toyota/tests/test_zss.py diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 5c8dc465d0..13b83ea839 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -143,6 +143,7 @@ class CarController(CarControllerBase): hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE + CS.zss_lateral_active = bool(CC.latActive) if len(CC.orientationNED) == 3: self.pitch.update(CC.orientationNED[1]) diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index a0c62b62d4..babd2135a6 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -57,6 +57,11 @@ class CarState(CarStateBase): self.gvc = 0.0 self.secoc_synchronization = None self.pre_collision_2 = {} + self.zss_lateral_active = False + self.zss_previous_active = False + self.zss_needs_offset = False + self.zss_offset = 0.0 + self.zss_last_timestamp = 0 def update(self, can_parsers) -> structs.CarState: cp = can_parsers[Bus.pt] @@ -225,6 +230,25 @@ class CarState(CarStateBase): self.distance_button = cp.vl["PCM_CRUISE_4"]["DISTANCE"] buttonEvents += create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + if self.CP.flags & ToyotaFlags.ZSS: + # ZSS support - credit to Erich! Align on each real lateral engagement. + # A repeated parser value is not a new sensor sample; fall back to stock. + active = self.zss_lateral_active + if active and not self.zss_previous_active: + self.zss_needs_offset = True + self.zss_previous_active = active + stamp = cp.ts_nanos.get("SECONDARY_STEER_ANGLE", {}).get("ZORRO_STEER", 0) + if cp.can_valid and stamp > self.zss_last_timestamp: + self.zss_last_timestamp = stamp + zorro = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"] + if math.isfinite(zorro): + if self.zss_needs_offset: + if abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro) > 1e-3: + self.zss_offset = zorro - ret.steeringAngleDeg + self.zss_needs_offset = False + elif abs(ret.steeringAngleDeg - (zorro - self.zss_offset)) < 4.0: + ret.steeringAngleDeg = zorro - self.zss_offset + ret.buttonEvents = buttonEvents timestamp, expiry = parser_expiry(cp_cam, "RSA1", "TSGN1") if timestamp > self.dashboard_limit.observation.observed_ns: @@ -249,6 +273,8 @@ class CarState(CarStateBase): if CP.flags & ToyotaFlags.LONG_FILTER: pt_messages.append(("SDSU", 50)) + if CP.flags & ToyotaFlags.ZSS: + pt_messages.append(("SECONDARY_STEER_ANGLE", math.nan)) return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0), diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 5b16dc0c79..c4a7644b7d 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -75,6 +75,8 @@ class CarInterface(CarInterfaceBase): smart_dsu = (candidate in (CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT) and 0x2FF in fingerprint[0] and any(fw.ecu == Ecu.eps and bytes(fw.fwVersion) == b"8965B47070\x00\x00\x00\x00\x00\x00" for fw in car_fw)) + if candidate in (CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT) and fingerprint.get(0, {}).get(0x23) == 8: + ret.flags |= int(ToyotaFlags.ZSS) stop_and_go = bool(ret.flags & ToyotaFlags.TSS2) # In TSS2 cars, the camera does long control diff --git a/opendbc_repo/opendbc/car/toyota/prius_longitudinal.py b/opendbc_repo/opendbc/car/toyota/prius_longitudinal.py index cdf0a58701..acedbc1178 100644 --- a/opendbc_repo/opendbc/car/toyota/prius_longitudinal.py +++ b/opendbc_repo/opendbc/car/toyota/prius_longitudinal.py @@ -16,7 +16,7 @@ def configured(cp): return (cp.brand == 'toyota' and cp.carFingerprint in (CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT) and cp.pcmCruise and not cp.dashcamOnly and not cp.notCar and not cp.passive and cp.steerControlType == structs.CarParams.SteerControlType.torque and - cp.flags == int(ToyotaFlags.HYBRID | ToyotaFlags.LONG_FILTER | ToyotaFlags.RAISED_ACCEL_LIMIT) and + cp.flags & ~int(ToyotaFlags.ZSS) == int(ToyotaFlags.HYBRID | ToyotaFlags.LONG_FILTER | ToyotaFlags.RAISED_ACCEL_LIMIT) and len(cp.safetyConfigs) == 1 and cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.toyota and cp.safetyConfigs[0].safetyParam in (4169, 4681) and cp.openpilotLongitudinalControl == (cp.safetyConfigs[0].safetyParam == 4169)) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_zss.py b/opendbc_repo/opendbc/car/toyota/tests/test_zss.py new file mode 100644 index 0000000000..7f706e6c6f --- /dev/null +++ b/opendbc_repo/opendbc/car/toyota/tests/test_zss.py @@ -0,0 +1,81 @@ +"""Prius secondary-angle selection through the real CAN parser and controller.""" +import unittest +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, ToyotaFlags +from opendbc.car.toyota.prius_longitudinal import configured + + +def factory(car=CAR.TOYOTA_PRIUS, bus=0, length=8): + fp = {0: {}, 1: {}, 2: {}} + fp[bus][0x23] = length + return CarInterface.get_params(car, fp, [], False, False, False) + + +class TestZssCaller(unittest.TestCase): + def test_factory_identity_and_exact_presence(self): + for car in (CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT): + self.assertTrue(factory(car).flags & (1 << 16), "Detected Prius ZSS has no selected sensor owner") + for bus, length in ((1, 8), (2, 8), (0, 7)): + self.assertFalse(factory(car, bus, length).flags & ToyotaFlags.ZSS) + for car in (CAR.TOYOTA_AVALON, CAR.TOYOTA_COROLLA_TSS2, CAR.TOYOTA_RAV4_TSS2): + self.assertFalse(factory(car).flags & ToyotaFlags.ZSS) + + def test_real_interface_parser_apply_alignment_and_stale_fallback(self): + cp = factory() + ci = CarInterface(cp) + packer = CANPacker(DBC[cp.carFingerprint][Bus.pt]) + command = structs.CarControl() + + def update(tick, zorro): + frames = [] + if zorro is not None: + frames.append(packer.make_can_msg('SECONDARY_STEER_ANGLE', 0, {'ZORRO_STEER': zorro})) + frames += [packer.make_can_msg('STEER_TORQUE_SENSOR', 0, {'STEER_ANGLE': 10.}), + packer.make_can_msg('STEER_ANGLE_SENSOR', 0, {'STEER_ANGLE': 10.})] + for name in ('VSC1S07', 'BODY_CONTROL_STATE', 'BODY_CONTROL_STATE_2', 'BRAKE_MODULE', 'ESP_CONTROL', + 'GEAR_PACKET', 'PCM_CRUISE_SM', 'BLINKERS_STATE', 'PRE_COLLISION', 'WHEEL_SPEEDS', 'LIGHT_STALK'): + frames.append(packer.make_can_msg(name, 0, {})) + frames.extend([packer.make_can_msg('EPS_STATUS', 0, {'LKA_STATE': 1}), + packer.make_can_msg('PCM_CRUISE', 0, {'GAS_RELEASED': 1, 'CRUISE_ACTIVE': 1}), + packer.make_can_msg('PCM_CRUISE_2', 0, {'MAIN_ON': 1}), + packer.make_can_msg('LKAS_HUD', 2, {}), packer.make_can_msg('RSA1', 2, {}), + packer.make_can_msg('ACC_CONTROL', 0, {})]) + return ci.update([(2_000_000_000 + tick * 10_000_000, frames)]) + for tick in range(-100, 1): + state = update(tick, 20.) + self.assertTrue(state.canValid) + stock_angle = state.steeringAngleDeg + command.latActive = True + ci.apply(command.as_reader(), 2_000_000_001) + self.assertTrue(ci.CS.zss_lateral_active) + update(1, 20.) + self.assertFalse(ci.CS.zss_needs_offset) + aligned = update(2, 20.5) + self.assertAlmostEqual(aligned.steeringAngleDeg, stock_angle + .5, places=1) + stale = update(3, None) + self.assertAlmostEqual(stale.steeringAngleDeg, stock_angle, places=1) + jumped = update(4, 25.) + self.assertAlmostEqual(jumped.steeringAngleDeg, stock_angle, places=1) + command.latActive = False + ci.apply(command.as_reader(), 2_050_000_000) + update(5, 25.) + command.latActive = True + ci.apply(command.as_reader(), 2_060_000_000) + update(6, 25.) + self.assertAlmostEqual(update(7, 25.5).steeringAngleDeg, stock_angle + .5, places=1) + + def test_filter_ownership_accepts_sensor_without_changing_native_word(self): + from opendbc.car.toyota.prius_longitudinal import prepare_stock + rack = structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.eps, + fwVersion=b'8965B47070' + bytes(6)) + cp = CarInterface.get_params(CAR.TOYOTA_PRIUS, {0: {0x23: 8, 0x2FF: 8}, 1: {}, 2: {}}, [rack], False, False, False) + self.assertTrue(configured(cp)) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 4169) + sibling = cp.as_reader().as_builder() + sibling.flags |= 1 << 17 + self.assertFalse(configured(sibling)) + self.assertTrue(prepare_stock(cp)) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 4681) + self.assertTrue(cp.flags & ToyotaFlags.ZSS) diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 4692b4f16a..7c3bff85b6 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -79,6 +79,7 @@ class ToyotaFlags(IntFlag): RAISED_ACCEL_LIMIT = 1024 SECOC = 2048 AUTO_BRAKE_HOLD = 4096 + ZSS = 1 << 16 # Detected Prius secondary steering sensor; host-only, no safetyParam allocation # deprecated flags # these cars are speculated to allow stop and go when the DSU is unplugged