Merge branch 'brand/toyota/zss' into pre-toyota

opendbc_repo/opendbc/car/__init__.py

@@ -97,6 +97,7 @@ class Bus(StrEnum):
   party = auto()
   ap_party = auto()
 
+  zss = auto()
 
 def apply_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS):
 

opendbc_repo/opendbc/car/toyota/carstate.py

@@ -53,6 +53,9 @@ class CarState(CarStateBase):
     self.gvc = 0.0
     self.secoc_synchronization = None
 
+    from opendbc.car.toyota.zss import ZSS
+    self.zss = ZSS(CP.flags)
+
   def update(self, can_parsers) -> structs.CarState:
     cp = can_parsers[Bus.pt]
     cp_cam = can_parsers[Bus.cam]
@@ -100,6 +103,14 @@ class CarState(CarStateBase):
     ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
     torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
 
+    if self.zss.enabled:
+      self.zss.set_values(can_parsers[Bus.zss])
+      if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
+        main_on = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
+      else:
+        main_on = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
+      ret.steeringAngleDeg = self.zss.get_steering_angle_deg(main_on, cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"], ret.steeringAngleDeg)
+
     # On some cars, the angle measurement is non-zero while initializing
     if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]):
       self.accurate_steer_angle_seen = True
@@ -268,7 +279,12 @@ class CarState(CarStateBase):
           ("PRE_COLLISION", 33),
         ]
 
-    return {
+    parsers = {
       Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
       Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
     }
+
+    if CP.flags & ToyotaFlags.ZSS:
+      parsers[Bus.zss] = CANParser("toyota_zss", [("SECONDARY_STEER_ANGLE", 0)], 0)
+
+    return parsers

opendbc_repo/opendbc/car/toyota/interface.py

@@ -57,13 +57,22 @@ class CarInterface(CarInterfaceBase):
     if Ecu.hybrid in found_ecus:
       ret.flags |= ToyotaFlags.HYBRID.value
 
+    if 0x23 in fingerprint[0]:
+      print("----------------------------------------------")
+      print("dragonpilot: ZSS detected!")
+      print("----------------------------------------------")
+      ret.flags |= ToyotaFlags.ZSS.value
+
     if candidate == CAR.TOYOTA_PRIUS:
       stop_and_go = True
       # Only give steer angle deadzone to for bad angle sensor prius
       for fw in car_fw:
         if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
-          ret.steerActuatorDelay = 0.25
-          CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
+          if ret.flags & ToyotaFlags.ZSS.value:
+            CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+          else:
+            ret.steerActuatorDelay = 0.25
+            CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
 
     elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2):
       stop_and_go = True

opendbc_repo/opendbc/car/toyota/values.py

@@ -83,6 +83,7 @@ class ToyotaFlags(IntFlag):
   DOOR_AUTO_LOCK_UNLOCK = 2 ** 13
   TSS1_SNG = 2 ** 14
   RADAR_FILTER = 2 ** 15
+  ZSS = 2 ** 16
 
 class Footnote(Enum):
   CAMRY = CarFootnote(

opendbc_repo/opendbc/car/toyota/zss.py

@@ -0,0 +1,79 @@
+from opendbc.car.toyota.values import ToyotaFlags
+from opendbc.can.parser import CANParser
+
+ANGLE_DIFF_THRESHOLD = 4.0
+THRESHOLD_COUNT = 10
+
+class ZSS:
+
+  def __init__(self, flags: int):
+    self.enabled = flags & ToyotaFlags.ZSS.value
+    self._alka_enabled = flags & ToyotaFlags.ALKA.value
+
+    self._offset_compute_once = True
+    self._alka_active_prev = False
+    self._cruise_active_prev = False
+    self._offset = 0.
+    self._threshold_count = 0
+    self._zss_value = 0.
+    self._print_allow = True
+
+  def set_values(self, cp: CANParser):
+    self._zss_value = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
+
+  def get_enabled(self):
+    return self.enabled
+
+  def get_steering_angle_deg(self, main_on, cruise_active, stock_steering_angle_deg):
+    # off, fall back to stock
+    if not self.enabled:
+      return stock_steering_angle_deg
+
+    # when lka control is off, use stock
+    alka_active = self._is_alka_active(main_on)
+    if not cruise_active and not alka_active:
+      return stock_steering_angle_deg
+
+    # lka just activated
+    if not self._offset_compute_once and ((alka_active and not self._alka_active_prev) or (cruise_active and not self._cruise_active_prev)):
+      self._threshold_count = 0
+      self._offset_compute_once = True
+      self._print_allow = True
+
+    self._alka_active_prev = alka_active
+    self._cruise_active_prev = cruise_active
+
+    # compute offset when required
+    if self._offset_compute_once:
+      self._offset_compute_once = not self._compute_offset(stock_steering_angle_deg)
+
+    # error checking
+    if self._threshold_count >= THRESHOLD_COUNT:
+      if self._print_allow:
+        print("ZSS: Too many large diff, fallback to stock.")
+        self._print_allow = False
+      return stock_steering_angle_deg
+
+    if self._offset_compute_once:
+      print("ZSS: Compute offset required, fallback to stock.")
+      return stock_steering_angle_deg
+
+    zss_steering_angle_deg = self._zss_value - self._offset
+    angle_diff = abs(stock_steering_angle_deg - zss_steering_angle_deg)
+    if angle_diff > ANGLE_DIFF_THRESHOLD:
+      print(f"ZSS: Diff too large ({angle_diff}), fallback to stock. ")
+      if self._is_alka_active(main_on) or cruise_active:
+        self._threshold_count += 1
+      return stock_steering_angle_deg
+
+    return zss_steering_angle_deg
+
+  def _is_alka_active(self, main_on):
+    return self._alka_enabled and main_on != 0
+
+  def _compute_offset(self, steering_angle_deg):
+    if abs(steering_angle_deg) > 1e-3 and abs(self._zss_value) > 1e-3:
+      self._offset = self._zss_value - steering_angle_deg
+      print(f"ZSS: offset computed: {self._offset}")
+      return True
+    return False

opendbc_repo/opendbc/dbc/toyota_zss.dbc

@@ -0,0 +1,4 @@
+BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
+ SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX
+
+CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";