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,