From 21103f8835f66c42507beffbc0035bfbb273931d Mon Sep 17 00:00:00 2001 From: sshane Date: Fri, 9 Jul 2021 22:55:47 -0700 Subject: [PATCH] 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 c0d4c7e69a29e58a13573dab5a88792559b965b6. * add back * always add * pass acc_type through * Update selfdrive/car/toyota/carstate.py Co-authored-by: Adeeb Shihadeh * Update selfdrive/car/toyota/carstate.py Co-authored-by: Adeeb Shihadeh * 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 old-commit-hash: 16e4f432bf592ad74dcd6085e55c01570d642757 --- opendbc | 2 +- selfdrive/car/toyota/carcontroller.py | 4 ++-- selfdrive/car/toyota/carstate.py | 29 +++++++++++++++++---------- selfdrive/car/toyota/toyotacan.py | 4 ++-- 4 files changed, 23 insertions(+), 16 deletions(-) diff --git a/opendbc b/opendbc index a76084eddc..80e7b94ae9 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit a76084eddc8294c2834b862407fda46df9b60624 +Subproject commit 80e7b94ae9f7451235bd802fec04d2f5e601083b diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d75bc5ced7..42c47f1b01 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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. diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 5854a3f360..952e39ce21 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 14bc407446..1bb41f1021 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/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,