fix (#153) corner radar display

add lf,lr,rf,rr detect box -> car
2025-04-21 22:29:28 +09:00 · 2025-04-21 22:29:28 +09:00 · f60101314d
commit f60101314d
parent 81cdecd7da
3 changed files with 61 additions and 36 deletions
--- a/opendbc_repo/opendbc/car/hyundai/carstate.py
+++ b/opendbc_repo/opendbc/car/hyundai/carstate.py
@ -72,7 +72,7 @@ class CarState(CarStateBase):
    self.tcs_info_373 = None
    self.mdps_info = {}
    self.steer_touch_info = {}
-    
+
    self.cruise_buttons_msg = None
    self.hda2_lfa_block_msg = None

@ -84,7 +84,7 @@ class CarState(CarStateBase):

    self.main_enabled = True if Params().get_int("AutoEngage") == 2 else False
    self.gear_shifter = GearShifter.drive # Gear_init for Nexo ?? unknown 21.02.23.LSW
-    
+
    self.totalDistance = 0.0
    self.speedLimitDistance = 0
    self.pcmCruiseGap = 0
@ -94,6 +94,11 @@ class CarState(CarStateBase):
    self.LFA_ICON = 0
    self.paddle_button_prev = 0

+    self.lf_distance = 0
+    self.rf_distance = 0
+    self.lr_distance = 0
+    self.rr_distance = 0
+
  def update(self, can_parsers) -> structs.CarState:
    cp = can_parsers[Bus.pt]
    cp_cam = can_parsers[Bus.cam]
@ -292,16 +297,16 @@ class CarState(CarStateBase):
    else:
      ret.speedLimit = 0
      ret.speedLimitDistance = 0
-      
+
    self.update_speed_limit(ret)

    if prev_main_buttons == 0 and self.main_buttons[-1] != 0:
      self.main_enabled = not self.main_enabled

    return ret
-  
+
  def update_speed_limit(self, ret):
-    self.totalDistance += ret.vEgo * DT_CTRL 
+    self.totalDistance += ret.vEgo * DT_CTRL
    if ret.speedLimit > 0 and not ret.gasPressed:
      if self.speedLimitDistance <= self.totalDistance:
        self.speedLimitDistance = self.totalDistance + ret.speedLimit * 6
@ -309,7 +314,7 @@ class CarState(CarStateBase):
    else:
      self.speedLimitDistance = self.totalDistance
    ret.speedLimitDistance = self.speedLimitDistance - self.totalDistance
-    
+
  def update_canfd(self, can_parsers) -> structs.CarState:
    cp = can_parsers[Bus.pt]
    cp_cam = can_parsers[Bus.cam]
@ -367,8 +372,8 @@ class CarState(CarStateBase):
      self.steer_touch_info = copy.copy(cp.vl["STEER_TOUCH_2AF"])

    # carrot test
-    left_blinker_lamp = cp.vl["BLINKERS"]["LEFT_LAMP"] or cp.vl["BLINKERS"]["LEFT_LAMP_ALT"] 
-    right_blinker_lamp = cp.vl["BLINKERS"]["RIGHT_LAMP"] or cp.vl["BLINKERS"]["RIGHT_LAMP_ALT"] 
+    left_blinker_lamp = cp.vl["BLINKERS"]["LEFT_LAMP"] or cp.vl["BLINKERS"]["LEFT_LAMP_ALT"]
+    right_blinker_lamp = cp.vl["BLINKERS"]["RIGHT_LAMP"] or cp.vl["BLINKERS"]["RIGHT_LAMP_ALT"]
    ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, left_blinker_lamp, right_blinker_lamp)

    # TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP']
@ -424,6 +429,10 @@ class CarState(CarStateBase):
          self.adrv_info_161 = copy.copy(cp_cam.vl.get("ADRV_0x161", {}))
        if "ADRV_0x162" in cp_cam.vl:
          self.adrv_info_162 = copy.copy(cp_cam.vl.get("ADRV_0x162", {}))
+          self.lf_distance = cp_cam.vl["CCNC_0x162"]["LF_DETECT_DISTANCE"]
+          self.rf_distance = cp_cam.vl["CCNC_0x162"]["RF_DETECT_DISTANCE"]
+          self.lr_distance = cp_cam.vl["CCNC_0x162"]["LR_DETECT_DISTANCE"]
+          self.rr_distance = cp_cam.vl["CCNC_0x162"]["RR_DETECT_DISTANCE"]
      if "ADRV_0x200" in cp_cam.vl:
        self.adrv_info_200 = copy.copy(cp_cam.vl.get("ADRV_0x200", {}))
      if "ADRV_0x1ea" in cp_cam.vl:
@ -436,7 +445,7 @@ class CarState(CarStateBase):
        if int(self.hda_info_4a3["NEW_SIGNAL_4"]) == 17:
          speedLimit = self.hda_info_4a3["SPEED_LIMIT"]
          ret.speedLimit = speedLimit if speedLimit < 255 else 0 # 안됨.. 고속화도로나 고속도로는....
-        
+
      if "NEW_MSG_4B4" in cp.vl:
        self.new_msg_4b4 = copy.copy(cp.vl.get("NEW_MSG_4B4", {}))

@ -466,7 +475,7 @@ class CarState(CarStateBase):

    self.cruise_buttons.extend(cruise_button)
    # }} carrot
-    
+

    if self.cruise_btns_msg_canfd in cp.vl:
      self.cruise_buttons_msg = copy.copy(cp.vl[self.cruise_btns_msg_canfd])
@ -547,7 +556,7 @@ class CarState(CarStateBase):
      pt_messages += [
        ("CRUISE_BUTTONS", 50)
      ]
-      
+
    if CP.extFlags & HyundaiExtFlags.STEER_TOUCH:
      pt_messages += [
        ("STEER_TOUCH_2AF", 10),
@ -576,7 +585,7 @@ class CarState(CarStateBase):
        #("NEW_MSG_4B4", 10),  # G80 hda2개조차량은 안나옴. 원래그런건지.. 어짜피 안쓰는데이터이니깐...
      ]
    #if CP.flags & HyundaiFlags.CANFD_HDA2 and CP.extFlags & HyundaiExtFlags.NAVI_CLUSTER.value and not (CP.extFlags & HyundaiExtFlags.SCC_BUS2.value):
-    # 어떤차는 bus2에 있음, 내차는 bus0에 있는데.... 이건 옆두부와 관련이 없나?   
+    # 어떤차는 bus2에 있음, 내차는 bus0에 있는데.... 이건 옆두부와 관련이 없나?
    #if CP.flags & HyundaiFlags.CANFD_HDA2:
    #  pt_messages.append(("CLUSTER_SPEED_LIMIT", 10))

@ -641,7 +650,7 @@ class CarState(CarStateBase):
      ("CGW4", 5),
      ("WHL_SPD11", 50),
      ("SAS11", 100),
-      ("TPMS11", 5), 
+      ("TPMS11", 5),
    ]
    if CP.flags & HyundaiFlags.CC_ONLY_CAR:
      pt_messages.remove(("TCS11", 100))
@ -682,7 +691,7 @@ class CarState(CarStateBase):
    else:
      pt_messages.append(("LVR12", 100))
      pt_messages.append(("LVR11", 100))
-      
+
    if CP.extFlags & HyundaiExtFlags.HAS_LFA_BUTTON.value:
      pt_messages.append(("BCM_PO_11", 50))

@ -709,7 +718,7 @@ class CarState(CarStateBase):
      if CP.extFlags & HyundaiExtFlags.HAS_SCC14.value:
        cam_messages += [
          ("SCC14", 50),
-        ]      
+        ]
      if CP.flags & HyundaiFlags.USE_FCA.value:
        cam_messages += [
          ("FCA11", 50),
--- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
@ -95,7 +95,7 @@ def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active,
  else:
    if CS.lfa_info is not None:
      values["LKA_ACTIVE"] = 1 if CS.lfa_info["STEER_REQ"] == 1 else 0
-      
+
  if frame % 1000 < 40:
    values["STEERING_COL_TORQUE"] += 100
  ret.append(packer.make_can_msg("MDPS", CAN.CAM, values))
@ -436,7 +436,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
          #ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.ECAN, values))
        ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
      """
-        
+

    if frame % 5 == 0:
      if CP.extFlags & HyundaiExtFlags.CANFD_161.value:
@ -445,7 +445,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
          cruise_enabled = CC.enabled
          lat_active = CC.latActive
          nav_active = hud_control.activeCarrot > 1
-          
+
          # hdpuse carrot
          hdp_use = int(Params().get("HDPuse"))
          hdp_active = False
@ -483,7 +483,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
          if values["ALERTS_2"] in [1, 2, 5]:
            values["ALERTS_2"] = 0
            values["DAW_ICON"] = 0
-            
+
          values["SOUNDS_2"] = 0  # 2: STEER중지 경고후에도 사운드가 나옴.
          values["SOUNDS_4"] = 0  # 차선변경알림? 에이 그냥0으로..

@ -537,13 +537,24 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
      if CS.adrv_info_162 is not None:
        values = CS.adrv_info_162
        if hud_control.leadDistance > 0:
-          values["FF_DETECT_POS"] = hud_control.leadDistance
+          values["FF_DISTANCE"] = hud_control.leadDistance
          #values["FF_DETECT"] = 11 if hud_control.leadRelSpeed > -0.1 else 12  # bicycle
          #values["FF_DETECT"] = 5 if hud_control.leadRelSpeed > -0.1 else 6 # truck
          ff_type = 3 if hud_control.leadRadar == 1 else 13
          values["FF_DETECT"] = ff_type if hud_control.leadRelSpeed > -0.1 else ff_type + 1
          #values["FF_DETECT_LAT"] = - hud_control.leadDPath

+        sensors = [
+          ('lf', 'LF_DETECT'),
+          ('rf', 'RF_DETECT'),
+          ('lr', 'LR_DETECT'),
+          ('rr', 'RR_DETECT')
+        ]
+
+        for sensor_key, detect_key in sensors:
+          distance = getattr(CS, f"{sensor_key}_distance")
+          if distance > 0:
+            values[detect_key] = 3 if distance > 30 else 4

        """
        values["FAULT_FCA"] = 0
@ -575,9 +586,9 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
      # ADAS 콤마연결하면.. 0번에서.. (카메라혹은 다른곳에서)
      # 카메라 콤마연결+롱컨개조 하면.. 2번에서 데이터가 나옴..(카메라혹은 ADAS)
      if frame % 10 == 0:
-        
+
        pass
-        
+
  return ret

 def create_adrv_messages(CP, packer, CAN, frame):
--- a/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc
+++ b/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc
@ -316,20 +316,23 @@ BO_ 354 ADRV_0x162: 32 CCNC
 SG_ SPEEDLIMIT_WEATHER : 48|4@1+ (1,0) [0|15] "" XXX
 SG_ VIBRATE : 52|1@0+ (1,0) [0|1] "" XXX
 SG_ FF_DETECT : 64|5@1+ (1,0) [0|31] "" XXX
- SG_ FF_DETECT_POS : 69|11@1+ (0.1,0) [0|2047] "m" XXX
- SG_ FF_DETECT_LAT : 80|7@1- (0.1,0) [0|127] "" XXX
+ SG_ FF_DISTANCE : 69|11@1+ (0.1,0) [0|204.7] "m" XXX
+ SG_ FF_LATERAL : 80|7@1+ (0.1,0) [0|127] "m" XXX
+ SG_ FF_DETECT_ALT : 88|5@1+ (1,0) [0|31] "" XXX
+ SG_ FF_DISTANCE_ALT : 93|11@1+ (0.1,0) [0|204.7] "m" XXX
+ SG_ FF_LATERAL_ALT : 104|7@1+ (0.1,0) [0|127] "m" XXX
 SG_ LF_DETECT : 112|5@1+ (1,0) [0|31] "" XXX
- SG_ LF_DETECT_POS : 117|10@1+ (0.1,0) [0|1023] "m" XXX
- SG_ LF_DETECT_LAT : 128|7@1+ (1,0) [0|127] "" XXX
+ SG_ LF_DETECT_DISTANCE : 117|11@1+ (0.1,0) [0|204.7] "m" XXX
+ SG_ LF_DETECT_LATERAL : 128|7@1+ (0.1,0) [0|127] "m" XXX
 SG_ RF_DETECT : 136|5@1+ (1,0) [0|31] "" XXX
- SG_ RF_DETECT_POS : 141|10@1+ (0.1,0) [0|1023] "m" XXX
- SG_ RF_DETECT_LAT : 152|7@1+ (1,0) [0|127] "" XXX
+ SG_ RF_DETECT_DISTANCE : 141|11@1+ (0.1,0) [0|204.7] "m" XXX
+ SG_ RF_DETECT_LATERAL : 152|7@1+ (0.1,0) [0|127] "m" XXX
 SG_ LR_DETECT : 163|5@1+ (1,0) [0|31] "" XXX
- SG_ LR_DETECT_POS : 168|9@1+ (1,0) [0|511] "" XXX
- SG_ LR_DETECT_LAT : 177|7@1+ (1,0) [0|127] "" XXX
+ SG_ LR_DETECT_DISTANCE : 168|9@1+ (0.1,0) [0|204.7] "m" XXX
+ SG_ LR_DETECT_LATERAL : 177|7@1+ (0.1,0) [0|127] "m" XXX
 SG_ RR_DETECT : 192|5@1+ (1,0) [0|31] "" XXX
- SG_ RR_DETECT_POS : 197|9@1+ (0.1,0) [0|511] "m" XXX
- SG_ RR_DETECT_LAT : 206|7@1+ (1,0) [0|127] "" XXX
+ SG_ RR_DETECT_DISTANCE : 197|9@1+ (0.1,0) [0|204.7] "m" XXX
+ SG_ RR_DETECT_LATERAL : 206|7@1+ (0.1,0) [0|127] "m" XXX
 SG_ FAULT_FSS : 213|3@1+ (1,0) [0|7] "" XXX
 SG_ FAULT_FCA : 216|3@1+ (1,0) [0|7] "" XXX
 SG_ FAULT_LSS : 219|3@1+ (1,0) [0|7] "" XXX
@ -945,10 +948,12 @@ VAL_ 354 SIGNS 0 "HIDDEN" 1 "PEDESTRIAN_CROSSING" 2 "SCHOOL_CROSSWALK" 8 "STOP"
 VAL_ 354 SPEEDLIMIT_FLASH 0 "HIDDEN" 1 "ERROR" 2 "NORMAL" 4 "RED";
 VAL_ 354 SPEEDLIMIT_WEATHER 0 "HIDDEN" 1 "RAIN" 2 "SNOW" 3 "RAIN+SNOW" 4 "TRAILER";
 VAL_ 354 VIBRATE 0 "NONE" 1 "VIBRATE";
-VAL_ 354 LEAD 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "GRAY CAR" 4 "WHITE CAR" 5 "GRAY TRUCK" 6 "WHITE TRUCK" 7 "GRAY PERSON" 8 "WHITE PERSON" 9 "GRAY BICYCLE" 10 "WHITE BICYCLE" 11 "GRAY MOTORCYCLE" 12 "WHITE MOTORCYCLE" 13 "DARK CONE" 14 "ORANGE CONE";
-VAL_ 354 LEAD_ALT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "DIM CONE" 4 "ORANGE CONE";
-VAL_ 354 LEAD_LEFT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "GRAY CAR" 4 "WHITE CAR" 5 "GRAY TRUCK" 6 "WHITE TRUCK" 7 "GRAY PERSON" 8 "WHITE PERSON" 9 "GRAY BICYCLE" 10 "WHITE BICYCLE" 11 "GRAY MOTORCYCLE" 12 "WHITE MOTORCYCLE" 13 "DARK CONE" 14 "ORANGE CONE";
-VAL_ 354 LEAD_RIGHT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "GRAY CAR" 4 "WHITE CAR" 5 "GRAY TRUCK" 6 "WHITE TRUCK" 7 "GRAY PERSON" 8 "WHITE PERSON" 9 "GRAY BICYCLE" 10 "WHITE BICYCLE" 11 "GRAY MOTORCYCLE" 12 "WHITE MOTORCYCLE" 13 "DARK CONE" 14 "ORANGE CONE";
+VAL_ 354 FF_DETECT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "GRAY CAR" 4 "WHITE CAR" 5 "GRAY TRUCK" 6 "WHITE TRUCK" 7 "GRAY PERSON" 8 "WHITE PERSON" 9 "GRAY BICYCLE" 10 "WHITE BICYCLE" 11 "GRAY MOTORCYCLE" 12 "WHITE MOTORCYCLE" 13 "DARK CONE" 14 "ORANGE CONE";
+VAL_ 354 FF_DETECT_ALT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "DIM CONE" 4 "ORANGE CONE";
+VAL_ 354 LF_DETECT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "GRAY CAR" 4 "WHITE CAR" 5 "GRAY TRUCK" 6 "WHITE TRUCK" 7 "GRAY PERSON" 8 "WHITE PERSON" 9 "GRAY BICYCLE" 10 "WHITE BICYCLE" 11 "GRAY MOTORCYCLE" 12 "WHITE MOTORCYCLE" 13 "DARK CONE" 14 "ORANGE CONE";
+VAL_ 354 LR_DETECT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "GRAY CAR" 4 "WHITE CAR" 5 "GRAY TRUCK" 6 "WHITE TRUCK" 7 "GRAY PERSON" 8 "WHITE PERSON" 9 "GRAY BICYCLE" 10 "WHITE BICYCLE" 11 "GRAY MOTORCYCLE" 12 "WHITE MOTORCYCLE" 13 "DARK CONE" 14 "ORANGE CONE";
+VAL_ 354 RF_DETECT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "GRAY CAR" 4 "WHITE CAR" 5 "GRAY TRUCK" 6 "WHITE TRUCK" 7 "GRAY PERSON" 8 "WHITE PERSON" 9 "GRAY BICYCLE" 10 "WHITE BICYCLE" 11 "GRAY MOTORCYCLE" 12 "WHITE MOTORCYCLE" 13 "DARK CONE" 14 "ORANGE CONE";
+VAL_ 354 RR_DETECT 0 "HIDDEN" 1 "GRAY BOX" 2 "WHITE BOX" 3 "GRAY CAR" 4 "WHITE CAR" 5 "GRAY TRUCK" 6 "WHITE TRUCK" 7 "GRAY PERSON" 8 "WHITE PERSON" 9 "GRAY BICYCLE" 10 "WHITE BICYCLE" 11 "GRAY MOTORCYCLE" 12 "WHITE MOTORCYCLE" 13 "DARK CONE" 14 "ORANGE CONE";
 VAL_ 354 FAULT_FSS 0 "HIDDEN" 1 "CHECK_FORWARD_SAFETY_SYSTEM" 2 "FORWARD_SAFETY_SYSTEM_LIMITED_CAMERA_OBSCURED" 3 "FORWARD_SAFETY_SYSTEM_LIMITED_RADAR_BLOCKED";
 VAL_ 354 FAULT_FCA 0 "HIDDEN" 1 "CHECK_FORWARD_SIDE_SAFETY_SYSTEM" 2 "FORWARD_SIDE_SAFETY_SYSTEM_LIMITED_CAMERA_OBSCURED" 3 "FORWARD_SIDE_SAFETY_SYSTEM_LIMITED_RADAR_BLOCKED";
 VAL_ 354 FAULT_LSS 0 "HIDDEN" 1 "CHECK_LANE_SAFETY_SYSTEM" 2 "LANE_SAFETY_SYSTEM_DISABLED_CAMERA_OBSCURED";