Toyota: handle models with permanent low speed lockout (#21512)

* ignore permanent low speed lockout

* should always check in case not seen

* use cp_cam

* not bus 0

* change signal name according to ref dbc

* bump

* we don't rely on this

* bump

* Revert "Toyota: debug logging for low speed lockout (#21526)"

This reverts commit 7f2ad50fed.

* add back

* always add

* pass acc_type through

* Update selfdrive/car/toyota/carstate.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Update selfdrive/car/toyota/carstate.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* only update acc_type if not enableDsu

* only add when using

* only for openpilot long cars

* only seen TSS2

* add todo

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

opendbc

@@ -1 +1 @@
-Subproject commit a76084eddc8294c2834b862407fda46df9b60624
+Subproject commit 80e7b94ae9f7451235bd802fec04d2f5e601083b

selfdrive/car/toyota/carcontroller.py

@@ -115,9 +115,9 @@ class CarController():
       if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
         can_sends.append(create_acc_cancel_command(self.packer))
       elif CS.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
 
     if frame % 2 == 0 and CS.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.

selfdrive/car/toyota/carstate.py

@@ -4,8 +4,7 @@ from opendbc.can.can_define import CANDefine
 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, NO_STOP_TIMER_CAR
-from selfdrive.swaglog import cloudlog
+from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR
 
 
 class CarState(CarStateBase):
@@ -20,7 +19,9 @@ class CarState(CarStateBase):
     self.needs_angle_offset = True
     self.accurate_steer_angle_seen = False
     self.angle_offset = 0.
-    self.logged_setme_vals = set()
+
+    self.low_speed_lockout = False
+    self.acc_type = 1
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -76,16 +77,20 @@ class CarState(CarStateBase):
     if self.CP.carFingerprint == CAR.LEXUS_IS:
       ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
       ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS
-      self.low_speed_lockout = False
     else:
       ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
       ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS
-      self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
 
-      val = cp_cam.vl["ACC_CONTROL"]["SET_ME_X01"]
-      if val not in self.logged_setme_vals and val != 1:
-        self.logged_setme_vals.add(val)
-        cloudlog.event("setme_vals", vals=self.logged_setme_vals, car=self.CP.carFingerprint)
+    if self.CP.carFingerprint in TSS2_CAR:
+      self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"]
+
+    # some TSS2 cars have low speed lockout permanently set, so ignore on those cars
+    # these cars are identified by an ACC_TYPE value of 2.
+    # TODO: it may be possible to avoid the lockout and gain stop and go if you
+    # send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
+    if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint != CAR.LEXUS_IS) or \
+       (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
+      self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
 
     self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
@@ -192,14 +197,16 @@ class CarState(CarStateBase):
     signals = [
       ("FORCE", "PRE_COLLISION", 0),
       ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0),
-      ("SET_ME_X01", "ACC_CONTROL", 1)
     ]
 
     # use steering message to check if panda is connected to frc
     checks = [
       ("STEERING_LKA", 42),
       ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent
-      ("ACC_CONTROL", 0),
     ]
 
+    if CP.carFingerprint in TSS2_CAR:
+      signals.append(("ACC_TYPE", "ACC_CONTROL", 0))
+      checks.append(("ACC_CONTROL", 33))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)

selfdrive/car/toyota/toyotacan.py

@@ -28,11 +28,11 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
-    "SET_ME_X01": 1,
+    "ACC_TYPE": acc_type,
     "DISTANCE": 0,
     "MINI_CAR": lead,
     "SET_ME_X3": 3,