From 4c488fb71afbb0d1c9422e3f0303f595bde2f801 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:01:33 -0500
Subject: [PATCH 01/34] merge ZSS to SA (#197)

* SnG hack

* Eased up Internet requirement

See...
https://github.com/commaai/openpilot/pull/1096

* VBATT CHARGING & Turn off charge after 3 hrs

* Prii ZSS w/ Prime-centric tuning, etc.

* Minimized custom fork warning

* Update interface.py

* Update thermald.py

* Update interface.py

* Update interface.py

* Update lane_planner.py

* ZSS Prius INDI Tune

Co-authored-by: ErichMoraga <33645296+ErichMoraga@users.noreply.github.com>
Co-authored-by: ErichMoraga <mailto:ejmoraga@gmail.com|ejmoraga@gmail.com>
Co-authored-by: d412k5t412 <36134180+d412k5t412@users.noreply.github.com>
---
 opendbc/toyota_prius_2017_pt_generated.dbc |  3 ++
 selfdrive/car/toyota/carcontroller.py      |  4 +--
 selfdrive/car/toyota/carstate.py           | 41 +++++++++-------------
 selfdrive/car/toyota/interface.py          | 10 +++---
 selfdrive/car/toyota/values.py             | 12 +++----
 selfdrive/thermald/thermald.py             |  0
 6 files changed, 33 insertions(+), 37 deletions(-)
 mode change 100755 => 100644 selfdrive/car/toyota/interface.py
 mode change 100755 => 100644 selfdrive/thermald/thermald.py

diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc
index e0a72212fbe8a9..ae7fc5a6002446 100644
--- a/opendbc/toyota_prius_2017_pt_generated.dbc
+++ b/opendbc/toyota_prius_2017_pt_generated.dbc
@@ -132,6 +132,9 @@ BO_ 614 STEERING_IPAS: 8 IPAS
  SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX
  SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
 
+BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
+ SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX
+
 BO_ 643 PRE_COLLISION: 7 DSU
  SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
  SG_ SET_ME_X00 : 15|8@0+ (1,0) [0|255] "" XXX
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index d36257d07a7275..3e1ce7df2115ab 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -90,8 +90,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
       pcm_cancel_cmd = 1
 
     # on entering standstill, send standstill request
-    if CS.out.standstill and not self.last_standstill:
-      self.standstill_req = True
+    # if CS.out.standstill and not self.last_standstill:
+    #   self.standstill_req = True
     if CS.pcm_acc_status != 8:
       # pcm entered standstill or it's disabled
       self.standstill_req = False
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index d4cc71640f7e5b..3e293d22bcc8e4 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -4,7 +4,7 @@
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_STOP_TIMER_CAR
+from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR, NO_STOP_TIMER_CAR
 
 
 class CarState(CarStateBase):
@@ -12,15 +12,8 @@ def __init__(self, CP):
     super().__init__(CP)
     can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR']
-
-    # All TSS2 car have the accurate sensor
-    self.accurate_steer_angle_seen = CP.carFingerprint in TSS2_CAR
-
-    # On NO_DSU cars but not TSS2 cars the cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
-    # is zeroed to where the steering angle is at start.
-    # Need to apply an offset as soon as the steering angle measurements are both received
-    self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR
     self.angle_offset = 0.
+    self.init_angle_offset = False
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -47,21 +40,18 @@ def update(self, cp, cp_cam):
 
     ret.standstill = ret.vEgoRaw < 0.001
 
-    # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
-    if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
-      self.accurate_steer_angle_seen = True
-
-    if self.accurate_steer_angle_seen:
-      ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
-
-      if self.needs_angle_offset:
-        angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-        if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
-          self.needs_angle_offset = False
-          self.angle_offset = ret.steeringAngle - angle_wheel
+    if self.CP.carFingerprint in TSS2_CAR:
+      ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
+    elif True:
+      # cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
+      # need to apply an offset as soon as the steering angle measurements are both received
+      ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
+      angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+      if not self.init_angle_offset:
+        self.init_angle_offset = True
+        self.angle_offset = ret.steeringAngle - angle_wheel
     else:
       ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-
     ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
     can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
@@ -135,7 +125,6 @@ def get_can_parser(CP):
       ("GAS_RELEASED", "PCM_CRUISE", 1),
       ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
       ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
-      ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0),
       ("TURN_SIGNALS", "STEERING_LEVERS", 3),   # 3 is no blinkers
       ("LKA_STATE", "EPS_STATUS", 0),
       ("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
@@ -162,8 +151,12 @@ def get_can_parser(CP):
       signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
       checks.append(("PCM_CRUISE_2", 33))
 
-    if CP.carFingerprint == CAR.PRIUS:
+    if CP.carFingerprint in NO_DSU_CAR:
+      signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
+
+    if True:
       signals += [("STATE", "AUTOPARK_STATUS", 0)]
+      signals += [("ZORRO_STEER", "SECONDARY_STEER_ANGLE", 0)]
 
     # add gas interceptor reading if we are using it
     if CP.enableGasInterceptor:
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
old mode 100755
new mode 100644
index 55ad5e9180770d..0037ca2711b4e4
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -61,7 +61,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       stop_and_go = True
       ret.safetyParam = 50  # see conversion factor for STEER_TORQUE_EPS in dbc file
       ret.wheelbase = 2.70
-      ret.steerRatio = 14.22   # unknown end-to-end spec
+      ret.steerRatio = 15.74   # unknown end-to-end spec
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
 
@@ -74,7 +74,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.init('indi')
         ret.lateralTuning.indi.innerLoopGain = 4.0
         ret.lateralTuning.indi.outerLoopGain = 3.0
-        ret.lateralTuning.indi.timeConstant = 1.0
+        ret.lateralTuning.indi.timeConstant = 0.1
         ret.lateralTuning.indi.actuatorEffectiveness = 1.0
         ret.steerActuatorDelay = 0.5
 
@@ -278,7 +278,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       stop_and_go = True
       ret.safetyParam = 73
       ret.wheelbase = 2.8702
-      ret.steerRatio = 16.0  # not optimized
+      ret.steerRatio = 16.0 # not optimized
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
@@ -319,12 +319,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.safetyParam = 73
       ret.wheelbase = 2.66
       ret.steerRatio = 14.7
-      tire_stiffness_factor = 0.444  # not optimized yet
+      tire_stiffness_factor = 0.444 # not optimized yet
       ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
       ret.lateralTuning.pid.kf = 0.00006
 
-    ret.steerRateCost = 1.
+    ret.steerRateCost = 0.5
     ret.centerToFront = ret.wheelbase * 0.44
 
     # TODO: get actual value, for now starting with reasonable value for
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 9c663af9c5e6e3..7dacf247e3aedb 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -81,27 +81,27 @@ class CAR:
     36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8
   }],
   CAR.PRIUS: [{
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+    35: 8, 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   },
   #2019 LE
   {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+    35: 8, 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   },
   # 2020 Prius Prime LE
   {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+    35: 8, 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   },
   #2020 Prius Prime Limited
   {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8, 2026: 8, 2027: 8, 2029: 8, 2030: 8, 2031: 8
+    35: 8, 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8, 2026: 8, 2027: 8, 2029: 8, 2030: 8, 2031: 8
   },
   #2020 Central Europe Prime
   {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 8, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8
+    35: 8, 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 767: 4, 800: 8, 810: 2, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 8, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8
   },
   #2017 German Prius
   {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1792: 8, 1767: 4, 1863: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996: 8, 1998: 8, 2002: 8, 2010: 8, 2015: 8, 2016: 8, 2018: 8, 2024: 8, 2026: 8, 2030: 8
+    35: 8, 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1792: 8, 1767: 4, 1863: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996: 8, 1998: 8, 2002: 8, 2010: 8, 2015: 8, 2016: 8, 2018: 8, 2024: 8, 2026: 8, 2030: 8
   }],
   #Corolla w/ added Pedal Support (512L and 513L)
   CAR.COROLLA: [{
diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
old mode 100755
new mode 100644

From b9f5cc6d84abf4c1e098a9181ffe0dd9fb86450e Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:03:36 -0500
Subject: [PATCH 02/34] revert chmod

---
 selfdrive/car/toyota/interface.py | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 mode change 100644 => 100755 selfdrive/car/toyota/interface.py

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
old mode 100644
new mode 100755

From e6c7ed26dfb0239d4769e8cfaade551f8d953c14 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:04:23 -0500
Subject: [PATCH 03/34] revert chmod

---
 selfdrive/thermald/thermald.py | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 mode change 100644 => 100755 selfdrive/thermald/thermald.py

diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
old mode 100644
new mode 100755

From 3860b54941a388f904da3ffc333a04094252208c Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:05:04 -0500
Subject: [PATCH 04/34] revert comment for standstill_req

---
 selfdrive/car/toyota/carcontroller.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 3e1ce7df2115ab..d36257d07a7275 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -90,8 +90,8 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
       pcm_cancel_cmd = 1
 
     # on entering standstill, send standstill request
-    # if CS.out.standstill and not self.last_standstill:
-    #   self.standstill_req = True
+    if CS.out.standstill and not self.last_standstill:
+      self.standstill_req = True
     if CS.pcm_acc_status != 8:
       # pcm entered standstill or it's disabled
       self.standstill_req = False

From f4d70d9bbd383525cea9281d53f281ede2533029 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:06:27 -0500
Subject: [PATCH 05/34] revert interface changes

---
 selfdrive/car/toyota/interface.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 0037ca2711b4e4..55ad5e9180770d 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -61,7 +61,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       stop_and_go = True
       ret.safetyParam = 50  # see conversion factor for STEER_TORQUE_EPS in dbc file
       ret.wheelbase = 2.70
-      ret.steerRatio = 15.74   # unknown end-to-end spec
+      ret.steerRatio = 14.22   # unknown end-to-end spec
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
 
@@ -74,7 +74,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.init('indi')
         ret.lateralTuning.indi.innerLoopGain = 4.0
         ret.lateralTuning.indi.outerLoopGain = 3.0
-        ret.lateralTuning.indi.timeConstant = 0.1
+        ret.lateralTuning.indi.timeConstant = 1.0
         ret.lateralTuning.indi.actuatorEffectiveness = 1.0
         ret.steerActuatorDelay = 0.5
 
@@ -278,7 +278,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       stop_and_go = True
       ret.safetyParam = 73
       ret.wheelbase = 2.8702
-      ret.steerRatio = 16.0 # not optimized
+      ret.steerRatio = 16.0  # not optimized
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
@@ -319,12 +319,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.safetyParam = 73
       ret.wheelbase = 2.66
       ret.steerRatio = 14.7
-      tire_stiffness_factor = 0.444 # not optimized yet
+      tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
       ret.lateralTuning.pid.kf = 0.00006
 
-    ret.steerRateCost = 0.5
+    ret.steerRateCost = 1.
     ret.centerToFront = ret.wheelbase * 0.44
 
     # TODO: get actual value, for now starting with reasonable value for

From 8114353c7d0e70cd5ebe9d19cbbd17af1b8d5b0a Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:13:31 -0500
Subject: [PATCH 06/34] revert all carstate changes

---
 selfdrive/car/toyota/carstate.py | 41 +++++++++++++++++++-------------
 1 file changed, 24 insertions(+), 17 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 3e293d22bcc8e4..d4cc71640f7e5b 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -4,7 +4,7 @@
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR, NO_STOP_TIMER_CAR
+from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_STOP_TIMER_CAR
 
 
 class CarState(CarStateBase):
@@ -12,8 +12,15 @@ def __init__(self, CP):
     super().__init__(CP)
     can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR']
+
+    # All TSS2 car have the accurate sensor
+    self.accurate_steer_angle_seen = CP.carFingerprint in TSS2_CAR
+
+    # On NO_DSU cars but not TSS2 cars the cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
+    # is zeroed to where the steering angle is at start.
+    # Need to apply an offset as soon as the steering angle measurements are both received
+    self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR
     self.angle_offset = 0.
-    self.init_angle_offset = False
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -40,18 +47,21 @@ def update(self, cp, cp_cam):
 
     ret.standstill = ret.vEgoRaw < 0.001
 
-    if self.CP.carFingerprint in TSS2_CAR:
-      ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
-    elif True:
-      # cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start.
-      # need to apply an offset as soon as the steering angle measurements are both received
-      ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
-      angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-      if not self.init_angle_offset:
-        self.init_angle_offset = True
-        self.angle_offset = ret.steeringAngle - angle_wheel
+    # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
+    if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
+      self.accurate_steer_angle_seen = True
+
+    if self.accurate_steer_angle_seen:
+      ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
+
+      if self.needs_angle_offset:
+        angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+        if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
+          self.needs_angle_offset = False
+          self.angle_offset = ret.steeringAngle - angle_wheel
     else:
       ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+
     ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
     can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
@@ -125,6 +135,7 @@ def get_can_parser(CP):
       ("GAS_RELEASED", "PCM_CRUISE", 1),
       ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
       ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
+      ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0),
       ("TURN_SIGNALS", "STEERING_LEVERS", 3),   # 3 is no blinkers
       ("LKA_STATE", "EPS_STATUS", 0),
       ("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
@@ -151,12 +162,8 @@ def get_can_parser(CP):
       signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
       checks.append(("PCM_CRUISE_2", 33))
 
-    if CP.carFingerprint in NO_DSU_CAR:
-      signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
-
-    if True:
+    if CP.carFingerprint == CAR.PRIUS:
       signals += [("STATE", "AUTOPARK_STATUS", 0)]
-      signals += [("ZORRO_STEER", "SECONDARY_STEER_ANGLE", 0)]
 
     # add gas interceptor reading if we are using it
     if CP.enableGasInterceptor:

From a215c0ddcae3808593e7fe7b1cc1af9e0def69d0 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:29:23 -0500
Subject: [PATCH 07/34] add code to detect ZSS

---
 cereal/car.capnp                  | 1 +
 selfdrive/car/toyota/carstate.py  | 5 +++++
 selfdrive/car/toyota/interface.py | 7 +++++++
 3 files changed, 13 insertions(+)

diff --git a/cereal/car.capnp b/cereal/car.capnp
index bd4d7c6536c992..f9aa4398e4bc1b 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -405,6 +405,7 @@ struct CarParams {
   communityFeature @46: Bool;  # true if a community maintained feature is detected
   fingerprintSource @49: FingerprintSource;
   networkLocation @50 :NetworkLocation;  # Where Panda/C2 is integrated into the car's CAN network
+  hasZSS @51 : Bool;  # true if ZSS is detected
 
   struct LateralParams {
     torqueBP @0 :List(Int32);
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index d4cc71640f7e5b..4b095fb63d3656 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -51,6 +51,9 @@ def update(self, cp, cp_cam):
     if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
       self.accurate_steer_angle_seen = True
 
+    if self.CP.hasZSS:
+      pass
+
     if self.accurate_steer_angle_seen:
       ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
 
@@ -164,6 +167,8 @@ def get_can_parser(CP):
 
     if CP.carFingerprint == CAR.PRIUS:
       signals += [("STATE", "AUTOPARK_STATUS", 0)]
+    if CP.hasZSS:
+      signals += [("ZORRO_STEER", "SECONDARY_STEER_ANGLE", 0)]
 
     # add gas interceptor reading if we are using it
     if CP.enableGasInterceptor:
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 55ad5e9180770d..22e44ae3ad36db 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -21,6 +21,13 @@ def compute_gb(accel, speed):
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):  # pylint: disable=dangerous-default-value
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
+    ret.hasZSS = 35 in fingerprint and fingerprint[35] == 8
+    if ret.hasZSS:
+      with open('/data/hasZSS.txt', 'a') as f:
+        f.write('has ZSS!\n')
+    else:
+      with open('/data/hasZSS.txt', 'a') as f:
+        f.write('does NOT have ZSS!\n')
 
     ret.carName = "toyota"
     ret.safetyModel = car.CarParams.SafetyModel.toyota

From bab75fcc4447bcf343265dd2fc1c81395c00d3c3 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:29:51 -0500
Subject: [PATCH 08/34] fix

---
 cereal/car.capnp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cereal/car.capnp b/cereal/car.capnp
index f9aa4398e4bc1b..53da0d3f718bdb 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -405,7 +405,7 @@ struct CarParams {
   communityFeature @46: Bool;  # true if a community maintained feature is detected
   fingerprintSource @49: FingerprintSource;
   networkLocation @50 :NetworkLocation;  # Where Panda/C2 is integrated into the car's CAN network
-  hasZSS @51 : Bool;  # true if ZSS is detected
+  hasZSS @51: Bool;  # true if ZSS is detected
 
   struct LateralParams {
     torqueBP @0 :List(Int32);

From b9ab317f26ec49565038ff46bf60f7817915328b Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:33:14 -0500
Subject: [PATCH 09/34] add ZSS code

---
 selfdrive/car/toyota/carstate.py | 35 ++++++++++++++++++--------------
 1 file changed, 20 insertions(+), 15 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 4b095fb63d3656..898814326991c3 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -21,6 +21,7 @@ def __init__(self, CP):
     # Need to apply an offset as soon as the steering angle measurements are both received
     self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR
     self.angle_offset = 0.
+    self.init_angle_offset = False
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -47,23 +48,27 @@ def update(self, cp, cp_cam):
 
     ret.standstill = ret.vEgoRaw < 0.001
 
-    # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
-    if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
-      self.accurate_steer_angle_seen = True
-
     if self.CP.hasZSS:
-      pass
-
-    if self.accurate_steer_angle_seen:
-      ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
-
-      if self.needs_angle_offset:
-        angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-        if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
-          self.needs_angle_offset = False
-          self.angle_offset = ret.steeringAngle - angle_wheel
+      ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
+      angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+      if not self.init_angle_offset:
+        self.init_angle_offset = True
+        self.angle_offset = ret.steeringAngle - angle_wheel
     else:
-      ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+      # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
+      if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
+        self.accurate_steer_angle_seen = True
+
+      if self.accurate_steer_angle_seen:
+        ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
+
+        if self.needs_angle_offset:
+          angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+          if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
+            self.needs_angle_offset = False
+            self.angle_offset = ret.steeringAngle - angle_wheel
+      else:
+        ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
 
     ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
     can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])

From a5297dff46ccbcc9eab22827576f1aac4e0f220e Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:35:35 -0500
Subject: [PATCH 10/34] use needs_angle_offset

---
 selfdrive/car/toyota/carstate.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 898814326991c3..573f2010fa7321 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -21,7 +21,6 @@ def __init__(self, CP):
     # Need to apply an offset as soon as the steering angle measurements are both received
     self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR
     self.angle_offset = 0.
-    self.init_angle_offset = False
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -51,8 +50,8 @@ def update(self, cp, cp_cam):
     if self.CP.hasZSS:
       ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
       angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-      if not self.init_angle_offset:
-        self.init_angle_offset = True
+      if self.needs_angle_offset:
+        self.needs_angle_offset = False
         self.angle_offset = ret.steeringAngle - angle_wheel
     else:
       # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero

From ee76f09c8b75cb9e920e1a38313b10c64a57548b Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:36:26 -0500
Subject: [PATCH 11/34] switch around for diff readability

---
 selfdrive/car/toyota/carstate.py | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 573f2010fa7321..9ea7ec9d6db244 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -47,13 +47,7 @@ def update(self, cp, cp_cam):
 
     ret.standstill = ret.vEgoRaw < 0.001
 
-    if self.CP.hasZSS:
-      ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
-      angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-      if self.needs_angle_offset:
-        self.needs_angle_offset = False
-        self.angle_offset = ret.steeringAngle - angle_wheel
-    else:
+    if not self.CP.hasZSS:
       # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
       if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
         self.accurate_steer_angle_seen = True
@@ -68,6 +62,12 @@ def update(self, cp, cp_cam):
             self.angle_offset = ret.steeringAngle - angle_wheel
       else:
         ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+    else:  # user has ZSS
+      ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
+      angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+      if self.needs_angle_offset:
+        self.needs_angle_offset = False
+        self.angle_offset = ret.steeringAngle - angle_wheel
 
     ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
     can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])

From 4834690b6d105ce10ab8503d2b7b7d8ba5af53b6 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:36:54 -0500
Subject: [PATCH 12/34] diff readability

---
 selfdrive/car/toyota/carstate.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 9ea7ec9d6db244..2d566507f562da 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -46,7 +46,6 @@ def update(self, cp, cp_cam):
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
 
     ret.standstill = ret.vEgoRaw < 0.001
-
     if not self.CP.hasZSS:
       # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
       if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:

From 9638a8cc37bdef34cba0e4c86a921f6128373d53 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:37:14 -0500
Subject: [PATCH 13/34] diff readability

---
 selfdrive/car/toyota/carstate.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 2d566507f562da..9ea7ec9d6db244 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -46,6 +46,7 @@ def update(self, cp, cp_cam):
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
 
     ret.standstill = ret.vEgoRaw < 0.001
+
     if not self.CP.hasZSS:
       # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
       if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:

From 3ed34546396d14a23c5e2aac4ff54547328fbb4c Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:49:19 -0500
Subject: [PATCH 14/34] test

---
 selfdrive/car/car_helpers.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index 9c6b97a0b4a6cd..fc2a601e0c784b 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -159,6 +159,7 @@ def fingerprint(logcan, sendcan, has_relay):
     source = car.CarParams.FingerprintSource.fixed
 
   cloudlog.warning("fingerprinted %s", car_fingerprint)
+  cloudlog.warning("fingerprint: {}".format(finger))
   return car_fingerprint, finger, vin, car_fw, source
 
 

From 5d039f5864f864433676c644d269998fb07ab98a Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 22:57:12 -0500
Subject: [PATCH 15/34] this should work

---
 selfdrive/car/car_helpers.py      | 1 -
 selfdrive/car/toyota/interface.py | 2 +-
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index fc2a601e0c784b..9c6b97a0b4a6cd 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -159,7 +159,6 @@ def fingerprint(logcan, sendcan, has_relay):
     source = car.CarParams.FingerprintSource.fixed
 
   cloudlog.warning("fingerprinted %s", car_fingerprint)
-  cloudlog.warning("fingerprint: {}".format(finger))
   return car_fingerprint, finger, vin, car_fw, source
 
 
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 22e44ae3ad36db..f89fbff6dced24 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -21,7 +21,7 @@ def compute_gb(accel, speed):
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):  # pylint: disable=dangerous-default-value
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
-    ret.hasZSS = 35 in fingerprint and fingerprint[35] == 8
+    ret.hasZSS = 35 in fingerprint and fingerprint[0][35] == 8
     if ret.hasZSS:
       with open('/data/hasZSS.txt', 'a') as f:
         f.write('has ZSS!\n')

From 9d12033514ca118e4649fbac6e3019a7baf1ff7a Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 23:00:48 -0500
Subject: [PATCH 16/34] move code

---
 selfdrive/car/toyota/interface.py | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index f89fbff6dced24..b336a02540542b 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -21,13 +21,6 @@ def compute_gb(accel, speed):
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):  # pylint: disable=dangerous-default-value
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
-    ret.hasZSS = 35 in fingerprint and fingerprint[0][35] == 8
-    if ret.hasZSS:
-      with open('/data/hasZSS.txt', 'a') as f:
-        f.write('has ZSS!\n')
-    else:
-      with open('/data/hasZSS.txt', 'a') as f:
-        f.write('does NOT have ZSS!\n')
 
     ret.carName = "toyota"
     ret.safetyModel = car.CarParams.SafetyModel.toyota
@@ -350,6 +343,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR
     # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
     ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
+    # Detect whether car has accurate ZSS
+    ret.hasZSS = 35 in fingerprint and fingerprint[0][35] == 8
+    if ret.hasZSS:
+      with open('/data/hasZSS.txt', 'a') as f:
+        f.write('has ZSS!\n')
+    else:
+      with open('/data/hasZSS.txt', 'a') as f:
+        f.write('does NOT have ZSS!\n')
     cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
     cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
     cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)

From 5448bcee189f144fca473bdded59ccb9a1c3aa61 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 23:02:41 -0500
Subject: [PATCH 17/34] ugh, fix

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index b336a02540542b..b3ba2e7809e414 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -344,7 +344,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
     ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
     # Detect whether car has accurate ZSS
-    ret.hasZSS = 35 in fingerprint and fingerprint[0][35] == 8
+    ret.hasZSS = 35 in fingerprint[0] and fingerprint[0][35] == 8
     if ret.hasZSS:
       with open('/data/hasZSS.txt', 'a') as f:
         f.write('has ZSS!\n')

From 238c733e586268218e2a7c8a625bc1eae031b168 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 23:03:15 -0500
Subject: [PATCH 18/34] this should work

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index b3ba2e7809e414..763efe3c2b1026 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -344,7 +344,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
     ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
     # Detect whether car has accurate ZSS
-    ret.hasZSS = 35 in fingerprint[0] and fingerprint[0][35] == 8
+    ret.hasZSS = 35 in fingerprint[0]
     if ret.hasZSS:
       with open('/data/hasZSS.txt', 'a') as f:
         f.write('has ZSS!\n')

From 586deef1ec084e9f3a68b7c6145783423ed7ab3d Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 23:07:19 -0500
Subject: [PATCH 19/34] use hex like the other fp checks

---
 selfdrive/car/toyota/interface.py | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 763efe3c2b1026..560e4f1f7d5fab 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -344,13 +344,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
     ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
     # Detect whether car has accurate ZSS
-    ret.hasZSS = 35 in fingerprint[0]
-    if ret.hasZSS:
-      with open('/data/hasZSS.txt', 'a') as f:
-        f.write('has ZSS!\n')
-    else:
-      with open('/data/hasZSS.txt', 'a') as f:
-        f.write('does NOT have ZSS!\n')
+    ret.hasZSS = 0x23 in fingerprint[0]
     cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
     cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
     cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)

From 7e7d954344de2117ba41ca9b606a39ae7388336f Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 4 Sep 2020 23:23:54 -0500
Subject: [PATCH 20/34] ZSS tuning

---
 selfdrive/car/toyota/interface.py | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 560e4f1f7d5fab..90a152d02cdb2f 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -27,6 +27,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
 
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
+    # Detect whether car has accurate ZSS
+    ret.hasZSS = 0x23 in fingerprint[0]
 
     ret.longitudinalTuning.deadzoneBP = [0., 9.]
     ret.longitudinalTuning.deadzoneV = [0., .15]
@@ -74,7 +76,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.init('indi')
         ret.lateralTuning.indi.innerLoopGain = 4.0
         ret.lateralTuning.indi.outerLoopGain = 3.0
-        ret.lateralTuning.indi.timeConstant = 1.0
+        if ret.hasZSS:
+          ret.lateralTuning.indi.timeConstant = 0.1
+        else:
+          ret.lateralTuning.indi.timeConstant = 1.0
         ret.lateralTuning.indi.actuatorEffectiveness = 1.0
         ret.steerActuatorDelay = 0.5
 
@@ -324,7 +329,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
       ret.lateralTuning.pid.kf = 0.00006
 
-    ret.steerRateCost = 1.
+    if ret.hasZSS:
+      ret.steerRateCost = 0.5
+    else:
+      ret.steerRateCost = 1.
     ret.centerToFront = ret.wheelbase * 0.44
 
     # TODO: get actual value, for now starting with reasonable value for
@@ -343,8 +351,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR
     # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
     ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
-    # Detect whether car has accurate ZSS
-    ret.hasZSS = 0x23 in fingerprint[0]
     cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
     cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
     cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)

From fc4762af2dd34e532912ec31846d9da500656226 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 18:54:52 -0500
Subject: [PATCH 21/34] revert to test

---
 selfdrive/car/toyota/carstate.py | 33 +++++++++++++-------------------
 1 file changed, 13 insertions(+), 20 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 9ea7ec9d6db244..97d824b6481937 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -47,27 +47,20 @@ def update(self, cp, cp_cam):
 
     ret.standstill = ret.vEgoRaw < 0.001
 
-    if not self.CP.hasZSS:
-      # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
-      if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
-        self.accurate_steer_angle_seen = True
-
-      if self.accurate_steer_angle_seen:
-        ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
-
-        if self.needs_angle_offset:
-          angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-          if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
-            self.needs_angle_offset = False
-            self.angle_offset = ret.steeringAngle - angle_wheel
-      else:
-        ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-    else:  # user has ZSS
-      ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
-      angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+    # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
+    if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
+      self.accurate_steer_angle_seen = True
+
+    if self.accurate_steer_angle_seen:
+      ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
+
       if self.needs_angle_offset:
-        self.needs_angle_offset = False
-        self.angle_offset = ret.steeringAngle - angle_wheel
+        angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
+        if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
+          self.needs_angle_offset = False
+          self.angle_offset = ret.steeringAngle - angle_wheel
+    else:
+      ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
 
     ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
     can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])

From 2dd8b2cc33e07646228daf4d06e77e4f4fb65a18 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 18:58:20 -0500
Subject: [PATCH 22/34] better implementation of getting ZSS

---
 selfdrive/car/toyota/carstate.py | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 97d824b6481937..3b6d47a0e8b171 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -51,12 +51,15 @@ def update(self, cp, cp_cam):
     if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
       self.accurate_steer_angle_seen = True
 
-    if self.accurate_steer_angle_seen:
-      ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
+    if self.accurate_steer_angle_seen or self.CP.hasZSS:
+      if self.CP.hasZSS:
+        ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
+      else:
+        ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
 
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-        if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
+        if (abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3) or self.CP.hasZSS:  # todo: is this required?
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
     else:

From d73ab14d5c5adf74d9e8a93011251f8930839251 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:00:21 -0500
Subject: [PATCH 23/34] better implementation of getting ZSS

---
 selfdrive/car/toyota/carstate.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 3b6d47a0e8b171..45cfed57536dcb 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -53,9 +53,10 @@ def update(self, cp, cp_cam):
 
     if self.accurate_steer_angle_seen or self.CP.hasZSS:
       if self.CP.hasZSS:
-        ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
+        steer_signals = 'SECONDARY_STEER_ANGLE', 'ZORRO_STEER'
       else:
-        ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
+        steer_signals = 'STEER_TORQUE_SENSOR', 'STEER_ANGLE'
+      ret.steeringAngle = cp.vl[steer_signals[0]][steer_signals[1]] - self.angle_offset
 
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']

From 0736b854019b801e126d8de26a65f7841cc9d1f2 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:01:23 -0500
Subject: [PATCH 24/34] revert

---
 selfdrive/car/toyota/carstate.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 45cfed57536dcb..3b6d47a0e8b171 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -53,10 +53,9 @@ def update(self, cp, cp_cam):
 
     if self.accurate_steer_angle_seen or self.CP.hasZSS:
       if self.CP.hasZSS:
-        steer_signals = 'SECONDARY_STEER_ANGLE', 'ZORRO_STEER'
+        ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
       else:
-        steer_signals = 'STEER_TORQUE_SENSOR', 'STEER_ANGLE'
-      ret.steeringAngle = cp.vl[steer_signals[0]][steer_signals[1]] - self.angle_offset
+        ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
 
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']

From 3c1f3ad7c28167b989d2e596d0c872d352ffb6ba Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:02:45 -0500
Subject: [PATCH 25/34] follow naming conventions

---
 cereal/car.capnp                  | 2 +-
 selfdrive/car/toyota/carstate.py  | 8 ++++----
 selfdrive/car/toyota/interface.py | 6 +++---
 3 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/cereal/car.capnp b/cereal/car.capnp
index 53da0d3f718bdb..8ff524ffe187ac 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -405,7 +405,7 @@ struct CarParams {
   communityFeature @46: Bool;  # true if a community maintained feature is detected
   fingerprintSource @49: FingerprintSource;
   networkLocation @50 :NetworkLocation;  # Where Panda/C2 is integrated into the car's CAN network
-  hasZSS @51: Bool;  # true if ZSS is detected
+  hasZss @51: Bool;  # true if ZSS is detected
 
   struct LateralParams {
     torqueBP @0 :List(Int32);
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 3b6d47a0e8b171..dfa4c60a665cd0 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -51,15 +51,15 @@ def update(self, cp, cp_cam):
     if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
       self.accurate_steer_angle_seen = True
 
-    if self.accurate_steer_angle_seen or self.CP.hasZSS:
-      if self.CP.hasZSS:
+    if self.accurate_steer_angle_seen or self.CP.hasZss:
+      if self.CP.hasZss:
         ret.steeringAngle = cp.vl["SECONDARY_STEER_ANGLE"]['ZORRO_STEER'] - self.angle_offset
       else:
         ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
 
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-        if (abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3) or self.CP.hasZSS:  # todo: is this required?
+        if (abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3) or self.CP.hasZss:  # todo: is this required?
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
     else:
@@ -167,7 +167,7 @@ def get_can_parser(CP):
 
     if CP.carFingerprint == CAR.PRIUS:
       signals += [("STATE", "AUTOPARK_STATUS", 0)]
-    if CP.hasZSS:
+    if CP.hasZss:
       signals += [("ZORRO_STEER", "SECONDARY_STEER_ANGLE", 0)]
 
     # add gas interceptor reading if we are using it
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 90a152d02cdb2f..665183da0babb2 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -28,7 +28,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
     # Detect whether car has accurate ZSS
-    ret.hasZSS = 0x23 in fingerprint[0]
+    ret.hasZss = 0x23 in fingerprint[0]
 
     ret.longitudinalTuning.deadzoneBP = [0., 9.]
     ret.longitudinalTuning.deadzoneV = [0., .15]
@@ -76,7 +76,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.init('indi')
         ret.lateralTuning.indi.innerLoopGain = 4.0
         ret.lateralTuning.indi.outerLoopGain = 3.0
-        if ret.hasZSS:
+        if ret.hasZss:
           ret.lateralTuning.indi.timeConstant = 0.1
         else:
           ret.lateralTuning.indi.timeConstant = 1.0
@@ -329,7 +329,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
       ret.lateralTuning.pid.kf = 0.00006
 
-    if ret.hasZSS:
+    if ret.hasZss:
       ret.steerRateCost = 0.5
     else:
       ret.steerRateCost = 1.

From 417da7cac4a3c533088d49eb3d142bdd2ff27597 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:03:55 -0500
Subject: [PATCH 26/34] simplify hasZss tuning

---
 selfdrive/car/toyota/interface.py | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 665183da0babb2..2c9dd57a889219 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -76,10 +76,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
         ret.lateralTuning.init('indi')
         ret.lateralTuning.indi.innerLoopGain = 4.0
         ret.lateralTuning.indi.outerLoopGain = 3.0
-        if ret.hasZss:
-          ret.lateralTuning.indi.timeConstant = 0.1
-        else:
-          ret.lateralTuning.indi.timeConstant = 1.0
+        ret.lateralTuning.indi.timeConstant = 0.1 if ret.hasZss else 1.0
         ret.lateralTuning.indi.actuatorEffectiveness = 1.0
         ret.steerActuatorDelay = 0.5
 
@@ -329,10 +326,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
       ret.lateralTuning.pid.kf = 0.00006
 
-    if ret.hasZss:
-      ret.steerRateCost = 0.5
-    else:
-      ret.steerRateCost = 1.
+    ret.steerRateCost = 0.5 if ret.hasZss else 1.0
     ret.centerToFront = ret.wheelbase * 0.44
 
     # TODO: get actual value, for now starting with reasonable value for

From 43d7772855f2b9f5639972048f59421227135a2b Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:05:03 -0500
Subject: [PATCH 27/34] one line

---
 selfdrive/car/toyota/interface.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 2c9dd57a889219..5a8f7bce93edf1 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -27,8 +27,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
 
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
-    # Detect whether car has accurate ZSS
-    ret.hasZss = 0x23 in fingerprint[0]
+    ret.hasZss = 0x23 in fingerprint[0]  # Detect whether car has accurate ZSS
 
     ret.longitudinalTuning.deadzoneBP = [0., 9.]
     ret.longitudinalTuning.deadzoneV = [0., .15]

From 76411d0eb3786fd7da17e42f5bdf084e93482d76 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:11:36 -0500
Subject: [PATCH 28/34] debug

---
 selfdrive/car/toyota/carstate.py | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index dfa4c60a665cd0..077baccfdfd89d 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -59,9 +59,14 @@ def update(self, cp, cp_cam):
 
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-        if (abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3) or self.CP.hasZss:  # todo: is this required?
+        if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
+          with open('/data/zss_offset', 'a') as f:
+            f.write('offset ZSS with {} ({}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel))
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
+        else:
+          with open('/data/zss_offset', 'a') as f:
+            f.write('did NOT offset ZSS with {} ({}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel))
     else:
       ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
 

From 789fd0478af0cc8ce283a240a342670d5e076f6a Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:12:07 -0500
Subject: [PATCH 29/34] rename

---
 selfdrive/car/toyota/carstate.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 077baccfdfd89d..a9fd395dad5bd3 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -60,12 +60,12 @@ def update(self, cp, cp_cam):
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
         if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
-          with open('/data/zss_offset', 'a') as f:
+          with open('/data/zss_offset.txt', 'a') as f:
             f.write('offset ZSS with {} ({}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel))
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
         else:
-          with open('/data/zss_offset', 'a') as f:
+          with open('/data/zss_offset.txt', 'a') as f:
             f.write('did NOT offset ZSS with {} ({}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel))
     else:
       ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']

From 4f16b06fd30a831f200dac3e79507230e970ae22 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:13:02 -0500
Subject: [PATCH 30/34] gather more data

---
 selfdrive/car/toyota/carstate.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index a9fd395dad5bd3..d913d759988cb6 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -61,12 +61,12 @@ def update(self, cp, cp_cam):
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
         if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
           with open('/data/zss_offset.txt', 'a') as f:
-            f.write('offset ZSS with {} ({}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel))
+            f.write('offset ZSS with {} ({}, {}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel, self.CP.hasZss))
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
         else:
           with open('/data/zss_offset.txt', 'a') as f:
-            f.write('did NOT offset ZSS with {} ({}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel))
+            f.write('offset ZSS with {} ({}, {}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel, self.CP.hasZss))
     else:
       ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
 

From 4fea493772725dd6195a42f5cc87a326afeed15e Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:44:34 -0500
Subject: [PATCH 31/34] remove debugging

---
 selfdrive/car/toyota/carstate.py | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index d913d759988cb6..34a0f4733bad56 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -60,13 +60,8 @@ def update(self, cp, cp_cam):
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
         if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
-          with open('/data/zss_offset.txt', 'a') as f:
-            f.write('offset ZSS with {} ({}, {}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel, self.CP.hasZss))
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
-        else:
-          with open('/data/zss_offset.txt', 'a') as f:
-            f.write('offset ZSS with {} ({}, {}, {})'.format(ret.steeringAngle - angle_wheel, ret.steeringAngle, angle_wheel, self.CP.hasZss))
     else:
       ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
 

From 21dfb2a872c69f2db381f71d8cad7bbbaa7656a2 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:46:17 -0500
Subject: [PATCH 32/34] just for safety

---
 selfdrive/car/toyota/carstate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 34a0f4733bad56..c6253dc5f80728 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -59,7 +59,7 @@ def update(self, cp, cp_cam):
 
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-        if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
+        if (abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3) or self.CP.hasZss:
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
     else:

From 3355727ee935d79af2ebb83912eb05c94f16cbef Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:47:20 -0500
Subject: [PATCH 33/34] this is probably better

---
 selfdrive/car/toyota/carstate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index c6253dc5f80728..514a24606ef558 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -59,7 +59,7 @@ def update(self, cp, cp_cam):
 
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-        if (abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3) or self.CP.hasZss:
+        if (abs(angle_wheel) > 1e-3 or self.CP.hasZss) and abs(ret.steeringAngle) > 1e-3:
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
     else:

From 777801db7c40a66ad3ae65a47e097622ac734dbe Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 5 Sep 2020 19:47:56 -0500
Subject: [PATCH 34/34] nah this is fine

---
 selfdrive/car/toyota/carstate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 514a24606ef558..c6253dc5f80728 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -59,7 +59,7 @@ def update(self, cp, cp_cam):
 
       if self.needs_angle_offset:
         angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
-        if (abs(angle_wheel) > 1e-3 or self.CP.hasZss) and abs(ret.steeringAngle) > 1e-3:
+        if (abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3) or self.CP.hasZss:
           self.needs_angle_offset = False
           self.angle_offset = ret.steeringAngle - angle_wheel
     else: