diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 4fe2df77b..9150156af 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -24,6 +24,8 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25) # - prolonged high driver torque: 17 (permanent) PERM_STEER_FAULTS = (3, 17) +ZSS_THRESHOLD = 4.0 +ZSS_THRESHOLD_COUNT = 10 class CarState(CarStateBase): def __init__(self, CP): @@ -50,8 +52,13 @@ class CarState(CarStateBase): self.lkas_hud = {} # FrogPilot variables + self.zss_compute = False + self.zss_cruise_active_last = False + self.pcm_accel_net = 0.0 self.pcm_neutral_force = 0.0 + self.zss_angle_offset = 0 + self.zss_threshold_count = 0 # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! def calculate_speed_limit(self, cp_cam, frogpilot_variables): @@ -218,6 +225,31 @@ class CarState(CarStateBase): self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"] self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] + # ZSS Support - Credit goes to the DragonPilot team! + if self.CP.flags & ToyotaFlags.ZSS and self.zss_threshold_count < ZSS_THRESHOLD_COUNT: + zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"] + + # Only compute ZSS offset when acc is active + zss_cruise_active = ret.cruiseState.available + if zss_cruise_active and not self.zss_cruise_active_last: + self.zss_compute = True # Cruise was just activated, so allow offset to be recomputed + self.zss_threshold_count = 0 + self.zss_cruise_active_last = zss_cruise_active + + # Compute ZSS offset + if self.zss_compute: + if abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro_steer) > 1e-3: + self.zss_compute = False + self.zss_angle_offset = zorro_steer - ret.steeringAngleDeg + + # Error check + new_steering_angle_deg = zorro_steer - self.zss_angle_offset + if abs(ret.steeringAngleDeg - new_steering_angle_deg) > ZSS_THRESHOLD: + self.zss_threshold_count += 1 + else: + # Apply offset + ret.steeringAngleDeg = new_steering_angle_deg + return ret @staticmethod @@ -273,6 +305,8 @@ class CarState(CarStateBase): ("SDSU", 100), ] + messages += [("SECONDARY_STEER_ANGLE", 0)] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 53bcbaff5..a73880cc6 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -44,6 +44,9 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 + if 0x23 in fingerprint[0]: # Detect if ZSS is present + ret.flags |= ToyotaFlags.ZSS.value + ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 984eda44d..442465cb5 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -60,6 +60,7 @@ class ToyotaFlags(IntFlag): # these cars are speculated to allow stop and go when the DSU is unplugged or disabled with sDSU SNG_WITHOUT_DSU = 512 + ZSS = 1024 class Footnote(Enum): CAMRY = CarFootnote(