From 3527b47f6ddb42c086fa18bd2cb92d5e490c61c6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 22 Aug 2023 21:17:30 -0700 Subject: [PATCH] Toyota: prepare to disable radar (#29542) * try to disable radar * fix bug and bump panda * prep * always attempt longitudinal for testers * fix rav4 * send ACC_HUD * bump panda * revert * check for failure to disable * fix arg * bump to master * revert to master * comments only * correct check * carcontroller * something like this * or this * use flag * send PCS HUD * clean up * carstate checks * fix from test models * consistent old-commit-hash: 0aa6e2ce41113a8f129ab88762037254f2951f91 --- selfdrive/car/toyota/carcontroller.py | 8 ++++++-- selfdrive/car/toyota/carstate.py | 6 +++--- selfdrive/car/toyota/interface.py | 19 +++++++++++++++++-- selfdrive/car/toyota/toyotacan.py | 2 +- selfdrive/car/toyota/values.py | 1 + 5 files changed, 28 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index c625c2e493..b765d70b9a 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -6,7 +6,7 @@ from openpilot.selfdrive.car.toyota.toyotacan import create_steer_command, creat create_accel_command, create_acc_cancel_command, \ create_fcw_command, create_lta_steer_command from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ - MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, \ + MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker @@ -166,7 +166,7 @@ class CarController: hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) - if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu: + if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value): can_sends.append(create_fcw_command(self.packer, fcw_alert)) # *** static msgs *** @@ -174,6 +174,10 @@ class CarController: if self.frame % fr_step == 0 and self.CP.enableDsu and self.CP.carFingerprint in cars: can_sends.append(make_can_msg(addr, vl, bus)) + # keep radar disabled + if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: + can_sends.append([0x750, 0, b"\x0F\x02\x3E\x00\x00\x00\x00\x00", 0]) + new_actuators = actuators.copy() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 3297750a8d..c96df6f10f 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -130,7 +130,7 @@ class CarState(CarStateBase): cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp - if self.CP.carFingerprint in TSS2_CAR: + if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: if not (self.CP.flags & ToyotaFlags.SMART_DSU.value): self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"] ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"]) @@ -153,7 +153,7 @@ class CarState(CarStateBase): ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 - if not self.CP.enableDsu: + if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) if self.CP.enableBsm: @@ -201,7 +201,7 @@ class CarState(CarStateBase): if CP.enableBsm: messages.append(("BSM", 1)) - if CP.carFingerprint in RADAR_ACC_CAR: + if CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value: if not CP.flags & ToyotaFlags.SMART_DSU.value: messages += [ ("ACC_CONTROL", 33), diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index c78479a97e..89b328b6ae 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,9 +2,11 @@ from cereal import car from openpilot.common.conversions import Conversions as CV from panda import Panda +from panda.python import uds from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase EventName = car.CarEvent.EventName @@ -218,7 +220,12 @@ class CarInterface(CarInterfaceBase): use_sdsu = bool(ret.flags & ToyotaFlags.SMART_DSU) if candidate in RADAR_ACC_CAR: ret.experimentalLongitudinalAvailable = use_sdsu - use_sdsu = use_sdsu and experimental_long + + if not use_sdsu: + if experimental_long and False: # TODO: disabling radar isn't supported yet + ret.flags |= ToyotaFlags.DISABLE_RADAR.value + else: + use_sdsu = use_sdsu and experimental_long # openpilot longitudinal enabled by default: # - non-(TSS2 radar ACC cars) w/ smartDSU installed @@ -226,7 +233,8 @@ class CarInterface(CarInterfaceBase): # - TSS2 cars with camera sending ACC_CONTROL where we can block it # openpilot longitudinal behind experimental long toggle: # - TSS2 radar ACC cars w/ smartDSU installed - ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) + # - TSS2 radar ACC cars w/o smartDSU installed (disables radar) + ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR if not ret.openpilotLongitudinalControl: @@ -261,6 +269,13 @@ class CarInterface(CarInterfaceBase): return ret + @staticmethod + def init(CP, logcan, sendcan): + # disable radar if alpha longitudinal toggled on radar-ACC car without CAN filter/smartDSU + if CP.flags & ToyotaFlags.DISABLE_RADAR.value: + communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL]) + disable_ecu(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control) + # returns a car.CarState def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 0c3330efa2..0792ac869f 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -56,7 +56,7 @@ def create_acc_cancel_command(packer): def create_fcw_command(packer, fcw): values = { - "PCS_INDICATOR": 1, + "PCS_INDICATOR": 1, # PCS turned off "FCW": fcw, "SET_ME_X20": 0x20, "SET_ME_X10": 0x10, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 7d62528748..bdde3c6095 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -42,6 +42,7 @@ class CarControllerParams: class ToyotaFlags(IntFlag): HYBRID = 1 SMART_DSU = 2 + DISABLE_RADAR = 4 class CAR: