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 ce14efb28..478fc0131 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 @@ -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 diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index b3ea685ca..4566bc5a1 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 ecf5990d3..f27b0189f 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/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( 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";