diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 29360c2039718b..a1a09047187b97 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -27,6 +27,8 @@ # - 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): @@ -49,6 +51,11 @@ def __init__(self, CP): # FrogPilot variables self.profile_restored = False + self.zss_compute = False + self.zss_cruise_active_last = False + + self.zss_angle_offset = 0 + self.zss_threshold_count = 0 self.traffic_signals = {} @@ -256,6 +263,30 @@ def update(self, cp, cp_cam, frogpilot_variables): SpeedLimitController.car_speed_limit = self.calculate_speed_limit(frogpilot_variables) SpeedLimitController.write_car_state() + # 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 def update_traffic_signals(self, cp_cam): @@ -329,6 +360,8 @@ def get_can_parser(CP): ("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 e1ec032cd1801d..6a45db5b552214 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -43,6 +43,9 @@ def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, 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 stop_and_go = candidate in TSS2_CAR diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 8fec1ee46627c9..0190aa9ed2be16 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -46,6 +46,7 @@ class ToyotaFlags(IntFlag): HYBRID = 1 SMART_DSU = 2 DISABLE_RADAR = 4 + ZSS = 8 RADAR_CAN_FILTER = 16