Janpoo6427/carrot wip trailer

opendbc_repo/opendbc/car/car.capnp

@@ -259,6 +259,7 @@ struct CarState {
   rightRearLongDist @78 :Float32; # rear-right corner radar longitudinal distance
   leftRearLatDist @79 :Float32; # rear-left corner radar lateral distance
   rightRearLatDist @80 :Float32; # rear-right corner radar lateral distance
+  trailerConnected @81 :Bool; # trailer connection state after disconnect debounce
 
   struct Tpms {
     fl @0 :Float32;

opendbc_repo/opendbc/car/hyundai/carstate.py

@@ -29,6 +29,7 @@
 GearShifter = structs.CarState.GearShifter
 
 READY_COUNT_OK = 200
+TRAILER_DISCONNECT_GRACE_FRAMES = int(5.0 / DT_CTRL)
 
 
 NUMERIC_TO_TZ = {
@@ -88,19 +89,19 @@ def __init__(self, CP):
     self.scc14 = None
     self.lkas11 = None
     self.clu11 = None
-    
+
     # for CANFD parsing
-    self.scc_control = None    
-    self.lfa = None    
-    self.lfa_alt = None    
-    self.lfahda_cluster = None    
+    self.scc_control = None
+    self.lfa = None
+    self.lfa_alt = None
+    self.lfahda_cluster = None
     self.adrv_0x161 = None
     self.adrv_0x200 = None
     self.adrv_0x1ea = None
     self.adrv_0x160 = None
-    self.ccnc_0x162 = None    
-    self.hda_info_4a3 = None    
-    self.tcs = None    
+    self.ccnc_0x162 = None
+    self.hda_info_4a3 = None
+    self.tcs = None
     self.mdps = None
     self.steer_touch_2af = None
     self.cruise_buttons_msg = None
@@ -146,7 +147,7 @@ def __init__(self, CP):
     if self.CP.openpilotLongitudinalControl and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC):
       ecu_disabled = True
 
-    
+
     self.HAS_LFA_BUTTON = True if 913 in fingerprints[0] else False
     self.CRUISE_BUTTON_ALT = True if 1007 in fingerprints[0] else False
 
@@ -160,12 +161,18 @@ def __init__(self, CP):
 
     self.cp_bsm = None
     self.time_zone = "UTC"
-    
+
     self.cp = None
     self.cp_cam = None
     self.cp_alt = None
     self.controls_ready_count = 0
 
+    # trailer detection
+    self.trailer_connected = False
+    self.trailer_timeout_cnt = 0
+    self.trailer_connected_prev = False
+    self.trailer_status = None
+
   def monitor_fingerprint(self, can_parsers, canfd):
     if self.controls_ready_count <= READY_COUNT_OK:
       if Params().get_bool("ControlsReady"):
@@ -192,7 +199,7 @@ def add_and_cache(parser, name: str, attr: str, ignore_counter: bool = False):
           setattr(self, attr, parser.vl[name])
           return True
         return False
-      
+
       if self.controls_ready_count == 50:
         self.cp.controls_ready = self.cp_cam.controls_ready = True
         if self.cp_alt is not None:
@@ -215,7 +222,7 @@ def add_and_cache(parser, name: str, attr: str, ignore_counter: bool = False):
           if not add_and_cache(self.cp_cam, "FCA11", "fca11"):
             add_and_cache(self.cp, "FCA11", "fca11")
           add_and_cache(self.cp_cam, "LKAS11", "lkas11")
-          add_and_cache(self.cp, "CLU11", "clu11")       
+          add_and_cache(self.cp, "CLU11", "clu11")
         elif self.controls_ready_count == 105:
           cp_cruise = self.cp_cam if self.CP.flags & HyundaiFlags.CAMERA_SCC else self.cp
           add_and_cache(cp_cruise, "SCC11", "scc11")
@@ -225,20 +232,20 @@ def add_and_cache(parser, name: str, attr: str, ignore_counter: bool = False):
       else: # canfd
         if self.controls_ready_count == 120:
           cp_cruise = self.cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else self.cp
-          add_and_cache(cp_cruise, "SCC_CONTROL", "scc_control")          
+          add_and_cache(cp_cruise, "SCC_CONTROL", "scc_control")
         elif self.controls_ready_count == 121:
           add_and_cache(self.cp, "TCS", "tcs")
           add_and_cache(self.cp, "MDPS", "mdps")
           add_and_cache(self.cp_cam, "LFA", "lfa")
-          add_and_cache(self.cp_cam, "LFA_ALT", "lfa_alt")          
+          add_and_cache(self.cp_cam, "LFA_ALT", "lfa_alt")
           add_and_cache(self.cp_cam, "LFAHDA_CLUSTER", "lfahda_cluster")
         elif self.controls_ready_count == 122:
-          add_and_cache(self.cp_cam, "ADRV_0x161", "adrv_0x161")  
+          add_and_cache(self.cp_cam, "ADRV_0x161", "adrv_0x161")
           add_and_cache(self.cp_cam, "ADRV_0x200", "adrv_0x200")
           add_and_cache(self.cp_cam, "ADRV_0x1ea", "adrv_0x1ea")
           add_and_cache(self.cp_cam, "ADRV_0x160", "adrv_0x160")
           add_and_cache(self.cp_cam, "CCNC_0x162", "ccnc_0x162")
-        elif self.controls_ready_count == 123:        
+        elif self.controls_ready_count == 123:
           add_and_cache(self.cp, "HDA_INFO_4A3", "hda_info_4a3")
           add_and_cache(self.cp, "STEER_TOUCH_2AF", "steer_touch_2af")
         elif self.controls_ready_count == 124:
@@ -256,11 +263,12 @@ def add_and_cache(parser, name: str, attr: str, ignore_counter: bool = False):
           add_and_cache(self.cp, "DOORS_SEATBELTS", "doors_seatbelts")
         elif self.controls_ready_count == 126:
           add_and_cache(self.cp, "CRUISE_BUTTONS_ALT2", "cruise_buttons_alt2", ignore_counter = True)
-
-
-
-
-
+          add_and_cache(self.cp, "TRAILER_STATUS", "trailer_status", ignore_counter = True)
+
+
+
+
+
   def update(self, can_parsers) -> structs.CarState:
     self.monitor_fingerprint(can_parsers, self.CP.flags & HyundaiFlags.CANFD)
     cp = can_parsers[Bus.pt]
@@ -504,7 +512,7 @@ def update_canfd(self, can_parsers) -> structs.CarState:
     if self.doors_seatbelts is not None:
       ret.doorOpen = self.doors_seatbelts["DRIVER_DOOR"] == 1
       ret.seatbeltUnlatched = self.doors_seatbelts["DRIVER_SEATBELT"] == 0
-        
+
     gear = cp.vl[self.gear_msg_canfd]["GEAR"] if not self.use_accelerator else 0 if self.accelerator is None else self.accelerator["GEAR"]
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
 
@@ -537,7 +545,28 @@ def update_canfd(self, can_parsers) -> structs.CarState:
       ret.steeringAngleDeg = cp.vl["MDPS"]["STEERING_ANGLE_2"] * -1
     else:
       ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
-
+
+    trailer_signal = self.trailer_status is not None and self.trailer_status["TRAILER_CONNECTED"] != 0
+    if trailer_signal:
+      # Rising edge is immediate so trailer-specific behavior is preserved.
+      self.trailer_timeout_cnt = 0
+      self.trailer_connected = True
+    elif self.trailer_connected:
+      # During ignition-off, the trailer bit can clear before the cluster shuts down.
+      # Keep suppression active through that transient, while still detecting a real
+      # disconnect after 5 seconds at the 100 Hz CarState update rate.
+      self.trailer_timeout_cnt += 1
+      if self.trailer_timeout_cnt > TRAILER_DISCONNECT_GRACE_FRAMES:
+        self.trailer_connected = False
+    else:
+      self.trailer_timeout_cnt = 0
+
+    ret.trailerConnected = self.trailer_connected
+
+    if self.trailer_connected != self.trailer_connected_prev:
+      print(f"[TRAILER_DEBUG] connected={self.trailer_connected} timeout={self.trailer_timeout_cnt}")
+      self.trailer_connected_prev = self.trailer_connected
+
     ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
     ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
     ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
@@ -576,7 +605,7 @@ def update_canfd(self, can_parsers) -> structs.CarState:
       self.MainMode_ACC = cp_cam.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
       self.ACCMode = cp_cam.vl["SCC_CONTROL"]["ACCMode"]
       self.LFA_ICON = cp_cam.vl["LFAHDA_CLUSTER"]["HDA_LFA_SymSta"]
-      
+
     if self.CP.openpilotLongitudinalControl:
       # These are not used for engage/disengage since openpilot keeps track of state using the buttons
       ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
@@ -618,7 +647,7 @@ def corner_max(signal):
         ret.leftBlindspot = True
       if right_block:
         ret.rightBlindspot = True
-        
+
     if self.hda_info_4a3 is not None:
       speedLimit = self.hda_info_4a3["SPEED_LIMIT"]
       if not self.is_metric:
@@ -729,6 +758,7 @@ def corner_max(signal):
                         *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})]
 
     self.paddle_button_prev = paddle_button
+
     return ret
 
   def get_can_parsers_canfd(self, CP):
@@ -738,6 +768,7 @@ def get_can_parsers_canfd(self, CP):
       msgs += [
         ("CRUISE_BUTTONS", 50)
       ]
+
     return {
       Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CanBus(CP).ECAN),
       Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).CAM),

opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py

@@ -127,7 +127,7 @@ def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active,
     if CS.lfa_alt is not None:
       values = copy.copy(CS.lfa_alt)
       rx_counter = values.pop("COUNTER", None)
-      if emergency_steering:      
+      if emergency_steering:
         pass
       else:
         #values = {} #CS.lfa_alt
@@ -164,7 +164,7 @@ def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active,
     values["HAS_LANE_SAFETY"] = 0
     values["LKA_ACTIVE"] = 0 # NEW_SIGNAL_1
 
-    values["DampingGain"] = 0 if lat_active else 100  
+    values["DampingGain"] = 0 if lat_active else 100
     #values["VALUE63"] = 0
 
     #values["VALUE82_SET256"] = 0
@@ -330,7 +330,7 @@ def create_lfa_icon_non_camera_scc(packer, CS, CAN, CC):
   return ret
 
 def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, hyundai_jerk, CS):
-  
+
   if CS.scc_control is None:
     return None
   enabled = (enabled or CS.softHoldActive > 0) and CS.paddle_button_prev == 0
@@ -341,7 +341,7 @@ def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, g
     acc_mode = 4 if enabled else 0
     enabled = False
     accel = accel_last = 0.5
-   
+
   elif hyundai_jerk.carrot_cruise == 2:
     accel = accel_last = hyundai_jerk.carrot_cruise_accel
 
@@ -399,7 +399,7 @@ def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, g
   # AccelLimitBandUpper, Lower
   values["SysFailState"] = 0    # 1: Performance degredation, 2: system temporairy unavailble, 3: SCC Service required , 눈이 묻어 레이더오류시... 2가 됨. 이때 가속을 안함...
 
-  values["AccelLimitBandUpper"] = 0.0   # 이값이 1.26일때 가속을 안하는 증상이 보임.. 
+  values["AccelLimitBandUpper"] = 0.0   # 이값이 1.26일때 가속을 안하는 증상이 보임..
   values["AccelLimitBandLower"] = 0.0
 
   values["ZEROS_7"] = 1
@@ -568,7 +568,7 @@ def alt_cruise_buttons(packer, CP, CAN, buttons, cruise_btns_msg, cnt):
   cruise_btns_msg["COUNTER"] = (cruise_btns_msg["COUNTER"] + 1 + cnt) % 256
   bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
   return packer.make_can_msg("CRUISE_BUTTONS_ALT", bus, cruise_btns_msg)
-  
+
 def hkg_can_fd_checksum(address: int, sig, d: bytearray) -> int:
   crc = 0
   for i in range(2, len(d)):
@@ -643,6 +643,12 @@ def _apply_radar_blink(values, radar_pairs, frame, *,
     values[det_key] = 2 - blink
     values[dist_key] = min_dist
 
+def _suppress_trailer_mode_warning(values, CS):
+  # Logs from IONIQ 9 show ALERTS_5=6 is the periodic
+  # "driver assistance limited in trailer mode" popup.
+  if CS.trailer_connected and values.get("ALERTS_5") == 6:
+    values["ALERTS_5"] = 0
+
 def _make_ccnc_values(values, CS, lat_active, frame, hud_control,
                      lane_line=True, corner_radar=True,
                      desire=0,
@@ -701,7 +707,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control,
         if  HDA_LFA_SymSta == 0 and 0 < frame % 200 < 12:
           values["LFA_BTN"] = 1
 
-        if CC.enabled:          
+        if CC.enabled:
           if not CS.MainMode_ACC:
             if 10 < frame % 200 <= 16 and CS.out.vEgo > 3.:
               values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
@@ -783,40 +789,51 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control,
           values["ALERTS_5"] = 0
 
         # curvature 표시(0x161쪽 기존 로직 유지)
+        _suppress_trailer_mode_warning(values, CS)
+
         curvature = round(CS.out.steeringAngleDeg / 3)
         values["LANELINE_CURVATURE"] = (min(abs(curvature), 15) + (-1 if curvature < 0 else 0)) if lat_active else 0
         values["LANELINE_CURVATURE_DIRECTION"] = 1 if curvature < 0 and lat_active else 0
 
-        lane_color = 6 if md is not None and md.meta.laneChangeAvailableLeft else 2
-        if lane_line_check >= 1:
-          lane_line_warn_left = CS.out.leftLaneLine % 10 not in (0, 5)   # 실선이면 주황
-        else:
-          lane_line_warn_left = CS.out.leftLaneLine // 10 == 2           # 노란색이면 주황
-        lane_color = 4 if lane_line_warn_left or CS.out.leftBlindspot else lane_color
-        if hud_control.leftLaneDepart:
-          values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1
+        trailer_lane_change_blocked = CS.trailer_connected
+        if trailer_lane_change_blocked:
+          values["LANELINE_LEFT"] = 2 if hud_control.leftLaneVisible else 0
+          values["LANELINE_RIGHT"] = 2 if hud_control.rightLaneVisible else 0
         else:
-          values["LANELINE_LEFT"] = lane_color if hud_control.leftLaneVisible else 0
+          lane_color = 6 if md is not None and md.meta.laneChangeAvailableLeft else 2
+          if lane_line_check >= 1:
+            lane_line_warn_left = CS.out.leftLaneLine % 10 not in (0, 5)
+          else:
+            lane_line_warn_left = CS.out.leftLaneLine // 10 == 2
+          lane_color = 4 if lane_line_warn_left or CS.out.leftBlindspot else lane_color
+          if hud_control.leftLaneDepart:
+            values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1
+          else:
+            values["LANELINE_LEFT"] = lane_color if hud_control.leftLaneVisible else 0
 
-        lane_color = 6 if md is not None and md.meta.laneChangeAvailableRight else 2
-        if lane_line_check >= 1:
-          lane_line_warn_right = CS.out.rightLaneLine % 10 not in (0, 5)
-        else:
-          lane_line_warn_right = CS.out.rightLaneLine // 10 == 2
-        lane_color = 4 if lane_line_warn_right or CS.out.rightBlindspot else lane_color
-        if hud_control.rightLaneDepart:
-          values["LANELINE_RIGHT"] = 4 if (frame // 50) % 2 == 0 else 1
-        else:
-          values["LANELINE_RIGHT"] = lane_color if hud_control.rightLaneVisible else 0
+          lane_color = 6 if md is not None and md.meta.laneChangeAvailableRight else 2
+          if lane_line_check >= 1:
+            lane_line_warn_right = CS.out.rightLaneLine % 10 not in (0, 5)
+          else:
+            lane_line_warn_right = CS.out.rightLaneLine // 10 == 2
+          lane_color = 4 if lane_line_warn_right or CS.out.rightBlindspot else lane_color
+          if hud_control.rightLaneDepart:
+            values["LANELINE_RIGHT"] = 4 if (frame // 50) % 2 == 0 else 1
+          else:
+            values["LANELINE_RIGHT"] = lane_color if hud_control.rightLaneVisible else 0
 
         values["LCA_LEFT_ARROW"] = 2 if CS.out.leftBlinker else 0
         values["LCA_RIGHT_ARROW"] = 2 if CS.out.rightBlinker else 0
 
-        values["LCA_LEFT_ICON"] = (1 if CS.out.leftBlindspot else 2) if lat_active else 0
-        values["LCA_RIGHT_ICON"] = (1 if CS.out.rightBlindspot else 2) if lat_active else 0
+        if trailer_lane_change_blocked:
+          values["LCA_LEFT_ICON"] = 1 if lat_active else 0
+          values["LCA_RIGHT_ICON"] = 1 if lat_active else 0
+        else:
+          values["LCA_LEFT_ICON"] = (1 if CS.out.leftBlindspot else 2) if lat_active else 0
+          values["LCA_RIGHT_ICON"] = (1 if CS.out.rightBlindspot else 2) if lat_active else 0
 
-        values["LANE_LEFT"] = 1 if desire in (1, 3) else 0
-        values["LANE_RIGHT"] = 1 if desire in (2, 4) else 0
+        values["LANE_LEFT"] = 0 if trailer_lane_change_blocked else 1 if desire in (1, 3) else 0
+        values["LANE_RIGHT"] = 0 if trailer_lane_change_blocked else 1 if desire in (2, 4) else 0
 
         ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values, rx_counter = rx_counter))