diff --git a/opendbc/car/__init__.py b/opendbc/car/__init__.py index 53a5d830dd..7029e0eac8 100644 --- a/opendbc/car/__init__.py +++ b/opendbc/car/__init__.py @@ -201,6 +201,8 @@ def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, fric friction = float(friction_interp) if friction_compensation else 0.0 return friction +def make_can_msg(addr, dat, bus): + return [addr, dat, bus] def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False): dat = [0x02, uds.SERVICE_TYPE.TESTER_PRESENT] diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index e208502821..ad01625dae 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -13,6 +13,8 @@ UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker +from opendbc.sunnypilot.car.toyota.ebsm import EnhancedBSMController + Ecu = structs.CarParams.Ecu LongCtrlState = structs.CarControl.Actuators.LongControlState SteerControlType = structs.CarParams.SteerControlType @@ -50,6 +52,8 @@ def __init__(self, dbc_names, CP): self.steer_rate_counter = 0 self.distance_button = 0 + self.enhanced_bsm_controller = EnhancedBSMController(CP) + self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL) self.accel_pid = PIDController(2.0, 0.5, 1 / (DT_CTRL * 3)) @@ -89,6 +93,9 @@ def update(self, CC, CS, now_nanos): # *** control msgs *** can_sends = [] + # Enhanced BSM integration + can_sends.extend(self.enhanced_bsm_controller.update(CS, self.frame)) + # *** handle secoc reset counter increase *** if self.CP.flags & ToyotaFlags.SECOC.value: if CS.secoc_synchronization['RESET_CNT'] != self.secoc_prev_reset_counter: diff --git a/opendbc/sunnypilot/car/toyota/__init__.py b/opendbc/sunnypilot/car/toyota/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc/sunnypilot/car/toyota/ebsm.py b/opendbc/sunnypilot/car/toyota/ebsm.py new file mode 100644 index 0000000000..4791956428 --- /dev/null +++ b/opendbc/sunnypilot/car/toyota/ebsm.py @@ -0,0 +1,150 @@ +from opendbc.car import structs +from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP, LEFT_BLINDSPOT, RIGHT_BLINDSPOT, create_set_bsm_debug_mode, create_bsm_polling_status + +class EnhancedBSMController: + def __init__(self, CP: structs.CarParams): + """ + Initialize Enhanced Blind Spot Monitoring Controller + + :param CP: Car Parameters + """ + self.CP = CP + + # Blindspot monitoring state tracking + self.left_blindspot_debug_enabled = False + self.right_blindspot_debug_enabled = False + self.last_blindspot_frame = 0 + + # New detection tracking variables + self._left_blindspot = False + self._left_blindspot_d1 = 0 + self._left_blindspot_d2 = 0 + self._left_blindspot_counter = 0 + + self._right_blindspot = False + self._right_blindspot_d1 = 0 + self._right_blindspot_d2 = 0 + self._right_blindspot_counter = 0 + + # Frame counter + self.frame = 0 + + def update(self, CS: structs.CarState, frame: int): + """ + Update method for Enhanced BSM + + :param CS: Car State + :param frame: Current frame number + :return: List of CAN messages for BSM + """ + # Check if Enhanced BSM is enabled via special flags + if not (self.CP.sunnypilotFlags & ToyotaFlagsSP.SP_ENHANCED_BSM): + return [] + + can_sends = [] + self.frame = frame + + # Process Left Blindspot + can_sends.extend(self._process_side_blindspot( + CS, + LEFT_BLINDSPOT, + self.left_blindspot_debug_enabled, + is_left=True + )) + + # Process Right Blindspot + can_sends.extend(self._process_side_blindspot( + CS, + RIGHT_BLINDSPOT, + self.right_blindspot_debug_enabled, + is_left=False + )) + + return can_sends + + def _process_side_blindspot(self, CS, side_marker, current_debug_state, is_left=True, + min_speed=6.0, debug_rate=20, always_on=True): + """ + Process blindspot monitoring for a specific side of the vehicle + + :param CS: Car State + :param side_marker: Byte marker for left or right side + :param current_debug_state: Current debug state for the side + :param is_left: Whether processing left or right side + :param min_speed: Minimum speed for BSM activation + :param debug_rate: Rate of debug message sending + :param always_on: Whether BSM should always be on + :return: List of CAN messages + """ + can_sends = [] + + # Activate BSM debug mode + if not current_debug_state: + if always_on or CS.out.vEgo > min_speed: + can_sends.append(create_set_bsm_debug_mode(side_marker, True)) + if is_left: + self.left_blindspot_debug_enabled = True + else: + self.right_blindspot_debug_enabled = True + + # Deactivate or poll BSM + else: + # Deactivate if not always on and no recent activity + if not always_on and self.frame - self.last_blindspot_frame > 50: + can_sends.append(create_set_bsm_debug_mode(side_marker, False)) + if is_left: + self.left_blindspot_debug_enabled = False + else: + self.right_blindspot_debug_enabled = False + + # Polling logic - alternate between left and right + poll_condition = (is_left and self.frame % debug_rate == 0) or \ + (not is_left and self.frame % debug_rate == debug_rate // 2) + + if poll_condition: + can_sends.append(create_bsm_polling_status(side_marker)) + self.last_blindspot_frame = self.frame + + return can_sends + + def sp_get_enhanced_bsm(self, cp): + """ + Enhanced Blind Spot Monitoring status retrieval + + :param cp: CAN parser + :return: Tuple of (left_blindspot, right_blindspot) + """ + # Let's keep all the commented out code for easy debug purposes in the future. + distance_1 = cp.vl["DEBUG"].get('BLINDSPOTD1') + distance_2 = cp.vl["DEBUG"].get('BLINDSPOTD2') + side = cp.vl["DEBUG"].get('BLINDSPOTSIDE') + + if all(val is not None for val in [distance_1, distance_2, side]): + if side == 65: # left blind spot + if distance_1 != self._left_blindspot_d1 or distance_2 != self._left_blindspot_d2: + self._left_blindspot_d1 = distance_1 + self._left_blindspot_d2 = distance_2 + self._left_blindspot_counter = 100 + self._left_blindspot = distance_1 > 10 or distance_2 > 10 + + elif side == 66: # right blind spot + if distance_1 != self._right_blindspot_d1 or distance_2 != self._right_blindspot_d2: + self._right_blindspot_d1 = distance_1 + self._right_blindspot_d2 = distance_2 + self._right_blindspot_counter = 100 + self._right_blindspot = distance_1 > 10 or distance_2 > 10 + + # update counters + self._left_blindspot_counter = max(0, self._left_blindspot_counter - 1) + self._right_blindspot_counter = max(0, self._right_blindspot_counter - 1) + + # reset blind spot status if counter reaches 0 + if self._left_blindspot_counter == 0: + self._left_blindspot = False + self._left_blindspot_d1 = self._left_blindspot_d2 = 0 + + if self._right_blindspot_counter == 0: + self._right_blindspot = False + self._right_blindspot_d1 = self._right_blindspot_d2 = 0 + + return self._left_blindspot, self._right_blindspot \ No newline at end of file diff --git a/opendbc/sunnypilot/car/toyota/values.py b/opendbc/sunnypilot/car/toyota/values.py new file mode 100644 index 0000000000..cf1ace9be1 --- /dev/null +++ b/opendbc/sunnypilot/car/toyota/values.py @@ -0,0 +1,19 @@ +from enum import IntFlag +from opendbc.car import make_can_msg + +class ToyotaFlagsSP(IntFlag): + """ + Special flags for Toyota SunnyPilot features + """ + SP_ENHANCED_BSM = 1 + +# Blindspot marker bytes +LEFT_BLINDSPOT = b"\x41" +RIGHT_BLINDSPOT = b"\x42" + +def create_set_bsm_debug_mode(lr_blindspot, enabled): + dat = b"\x02\x10\x60\x00\x00\x00\x00" if enabled else b"\x02\x10\x01\x00\x00\x00\x00" + dat = lr_blindspot + dat + return make_can_msg(0x750, dat, 0) +def create_bsm_polling_status(lr_blindspot): + return make_can_msg(0x750, lr_blindspot + b"\x02\x21\x69\x00\x00\x00\x00", 0) \ No newline at end of file