From b237bb085214d5f59dcfb31c10abb4cc4b1b8515 Mon Sep 17 00:00:00 2001
From: Rick Lan <ricklan@gmail.com>
Date: Fri, 28 Jun 2024 13:31:40 +0800
Subject: [PATCH] Toyota Enhanced BSM

---
 common/params.cc                      | 1 +
 selfdrive/car/toyota/carcontroller.py | 7 +++++++
 selfdrive/car/toyota/carstate.py      | 7 +++++++
 system/manager/manager.py             | 1 +
 4 files changed, 16 insertions(+)

diff --git a/common/params.cc b/common/params.cc
index 77cb677a1..6fa37d902 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -233,6 +233,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_toyota_pcm_compensation", PERSISTENT},
     {"dp_device_is_clone", PERSISTENT},
     {"dp_device_dm_unavailable", PERSISTENT},
+    {"dp_toyota_enhanced_bsm", PERSISTENT},
 };
 
 } // namespace
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index f45d1b775..7c4c80726 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -15,6 +15,9 @@ from openpilot.dp_ext.selfdrive.car.toyota.pcm_compensation_controller import PC
 # for pcm compensation
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
+# dp - for enhanced bsm
+from openpilot.dp_ext.selfdrive.car.toyota.bsm.controller import BSMController
+
 SteerControlType = car.CarParams.SteerControlType
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -51,6 +54,7 @@ class CarController(CarControllerBase):
     # dp
     self.dlc = DoorLockController()
     self.pcc = PCMCompensationController(CP, self.params, Params().get_bool("dp_toyota_pcm_compensation"))
+    self.bsmc = BSMController(self.CP)
 
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
@@ -63,6 +67,9 @@ class CarController(CarControllerBase):
     # *** control msgs ***
     can_sends = []
 
+    # dp - for enhanced bsm
+    can_sends = self.bsmc.get_can_sends(self.frame, CS.out.vEgo, can_sends)
+
     result = self.dlc.process(CS.out.gearShifter, CS.out.vEgo, CS.out.doorOpen)
     if result:
       can_sends.append(make_can_msg(result[0], result[1], result[2]))
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index c0be916b4..266a41939 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -12,6 +12,7 @@ from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_T
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
 
 from openpilot.dp_ext.selfdrive.car.toyota.zss_controller import ZSSController
+from openpilot.dp_ext.selfdrive.car.toyota.bsm.state import BSMState
 
 SteerControlType = car.CarParams.SteerControlType
 
@@ -55,6 +56,8 @@ class CarState(CarStateBase):
     self.zssc = ZSSController()
     # for pcm compensation
     self.pcm_neutral_force = 0.
+    # enhanced bsm
+    self.bsm_state = BSMState()
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -173,6 +176,10 @@ class CarState(CarStateBase):
       ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1)
       ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1)
 
+    # dp - enhanced bsm
+    self.bsm_state.update_states(ret.leftBlindspot, ret.rightBlindspot)
+    ret.leftBlindspot, ret.rightBlindspot = self.bsm_state.get_signals()
+
     if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
       self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
 
diff --git a/system/manager/manager.py b/system/manager/manager.py
index cf4599bb6..fde56b5f4 100755
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@@ -72,6 +72,7 @@ def manager_init() -> None:
     ("dp_toyota_pcm_compensation", "0"),
     ("dp_device_is_clone", "0"),
     ("dp_device_dm_unavailable", "0"),
+    ("dp_toyota_enhanced_bsm", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))