ZSS support

Co-Authored-By: Erich Moraga <33645296+erichmoraga@users.noreply.github.com>

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])

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),

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

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))

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)

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