From aa0a5e440b873b184b86b9a3ea3377277d252f38 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 12 May 2025 15:06:06 +0800 Subject: [PATCH] Toyota ZSS - init --- opendbc_repo/opendbc/car/__init__.py | 1 + opendbc_repo/opendbc/car/toyota/carstate.py | 18 ++++- opendbc_repo/opendbc/car/toyota/interface.py | 13 +++- opendbc_repo/opendbc/car/toyota/values.py | 1 + opendbc_repo/opendbc/car/toyota/zss.py | 79 ++++++++++++++++++++ opendbc_repo/opendbc/dbc/toyota_zss.dbc | 4 + 6 files changed, 113 insertions(+), 3 deletions(-) create mode 100644 opendbc_repo/opendbc/car/toyota/zss.py create mode 100644 opendbc_repo/opendbc/dbc/toyota_zss.dbc diff --git a/opendbc_repo/opendbc/car/__init__.py b/opendbc_repo/opendbc/car/__init__.py index dde58dd8c..7d7277cd8 100644 --- a/opendbc_repo/opendbc/car/__init__.py +++ b/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): diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 53f30709d..71226b369 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/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 @@ -263,7 +274,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 diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index b0f75ef7d..6ece62206 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 4848a354e..e9285e00a 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -77,6 +77,7 @@ class ToyotaFlags(IntFlag): SECOC = 2048 ALKA = 2 ** 12 + ZSS = 2 ** 13 class Footnote(Enum): CAMRY = CarFootnote( diff --git a/opendbc_repo/opendbc/car/toyota/zss.py b/opendbc_repo/opendbc/car/toyota/zss.py new file mode 100644 index 000000000..7d76f5158 --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/dbc/toyota_zss.dbc b/opendbc_repo/opendbc/dbc/toyota_zss.dbc new file mode 100644 index 000000000..551b83ab2 --- /dev/null +++ b/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";