From 1acbc2c55a641fce1bcc30c26ea8a6efc56d1ec1 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Tue, 22 Aug 2023 17:16:38 -0700
Subject: [PATCH] Toyota: prepare to disable radar (#29541)

* 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
old-commit-hash: 61653bf343bc189d9231234a5b95777998096fa6
---
 selfdrive/car/toyota/carcontroller.py | 5 +++++
 selfdrive/car/toyota/interface.py     | 9 +++++++++
 selfdrive/car/toyota/toyotacan.py     | 2 +-
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index c625c2e493..ab7e4b9d85 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -166,6 +166,7 @@ class CarController:
                                            hud_control.rightLaneVisible, hud_control.leftLaneDepart,
                                            hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
 
+      # TODO: send when disabling the radar to avoid a fault
       if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
         can_sends.append(create_fcw_command(self.packer, fcw_alert))
 
@@ -174,6 +175,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))
 
+    # uncomment to keep radar disabled, rate can be tuned:
+    # if self.frame % 20 == 0:
+    #   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/interface.py b/selfdrive/car/toyota/interface.py
index c78479a97e..6b8977f9f1 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -261,6 +261,15 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
+  # uncomment to disable radar:
+  # @staticmethod
+  # def init(CP, logcan, sendcan):
+  #   # a diagnostic mode of SESSION_TYPE.PROGRAMMING disabled the ECU on its own. car did not accept DISABLE_RX_DISABLE_TX, but it did not matter.
+  #   # a diagnostic mode of SESSION_TYPE.EXTENDED_DIAGNOSTIC and CONTROL_TYPE.ENABLE_RX_DISABLE_TX did work, so use that
+  #   # TODO: add a flag, radarUnavailable shouldn't be used as "radar acc" right now
+  #   if CP.openpilotLongitudinalControl and CP.radarUnavailable:
+  #     disable_ecu(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=b'\x28\x01\x01')
+
   # 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,