J3ff/IQ.Pilot
1
0
Fork 0
forked from IQ.Lvbs/IQ.Pilot
Code Pull Requests Activity
Files
release
IQ.Pilot/iqdbc_repo/iqdbc/car/hyundai/radar_interface.py
IQ.Lvbs history cleanup 9f9c9a70cc IQ.Pilot Prebuilt Release @ ab07000
2026-08-22 23:42:42 -05:00

886 lines
40 KiB
Python
Raw Permalink Blame History

import math
import os
from collections import deque
from iqdbc import DBC_PATH
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.interfaces import RadarInterfaceBase
from iqdbc.car.hyundai.values import DBC, HyundaiFlags, HyundaiExtFlags
from openpilot.common.params import Params
from iqdbc.car.hyundai.hyundaicanfd import CanBus
from openpilot.common.filter_simple import MyMovingAverage
SCC_TID = 0
RADAR_START_ADDR = 0x500
RADAR_MSG_COUNT = 32
RADAR_MSG_COUNT4 = 8
RADAR_GROUP4_MAX_LONG_DIST = 325.0
RADAR_GROUP4_MAX_YREL = 6.0
RADAR_START_ADDR_CANFD1 = 0x210
RADAR_MSG_COUNT1 = 16
RADAR_START_ADDR_CANFD2 = 0x3A5 # Group 2, Group 1: 0x210 2개씩?어???단 보류.
RADAR_MSG_COUNT2 = 32
RADAR_START_ADDR_CANFD3 = 0x400
RADAR_MSG_COUNT3 = 30
CORNER_OBJECT_235_START_ADDR = 0x235
CORNER_OBJECT_235_MSG_COUNT = 20
CORNER_OBJECT_235_TRACK_ID_OFFSET = 200
CORNER_OBJECT_235_DBC = 'hyundai_canfd_corner_radar_235_generated'
CORNER_OBJECT_180_START_ADDR = 0x180
CORNER_OBJECT_180_MSG_COUNT = 5
CORNER_OBJECT_180_SLOTS_PER_MSG = 2
CORNER_OBJECT_180_TRACK_ID_OFFSET = 240
CORNER_OBJECT_180_DBC = 'hyundai_canfd_corner_radar_180_generated'
CORNER_OBJECT_430_LEFT_START_ADDR = 0x430
CORNER_OBJECT_430_RIGHT_START_ADDR = 0x440
CORNER_OBJECT_430_MSG_COUNT_PER_SIDE = 8
CORNER_OBJECT_430_SLOTS_PER_MSG = 7
CORNER_OBJECT_430_TRACK_ID_OFFSET = 300
CORNER_OBJECT_430_DBC = 'hyundai_canfd_corner_radar_430_generated'
CORNER_OBJECT_430_EMPTY_RAW_VALUES = (0x010d1f40, 0x00010d1f)
CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MIN = 2520 # 126.0 m
CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MAX = 2600 # 130.0 m
CORNER_OBJECT_430_MAX_DREL = 120.0
CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE = 4
CORNER_OBJECT_430_DT = 0.05
CORNER_OBJECT_430_MAX_DREL_DELTA = 1.5
CORNER_OBJECT_430_CANDIDATE_META_BYTE_3 = (2,)
CORNER_OBJECT_430_CANDIDATE_EXCLUDED_SLOTS = (1,)
CORNER_OBJECT_430_CANDIDATE_RAW_DELTA = 200
CORNER_OBJECT_430_STRONG_META_BYTE_2 = (10,)
CORNER_OBJECT_430_WEAK_META_BYTE_2 = (5, 6, 7, 8, 9)
CORNER_OBJECT_430_STRONG_MIN_SUPPORT = 2
CORNER_OBJECT_430_WEAK_MIN_SUPPORT = 3
CORNER_OBJECT_430_CLUSTER_RAW_GAP = 200
CORNER_OBJECT_430_TRACK_MATCH_MAX_DREL_DELTA = 3.0
CORNER_OBJECT_430_MAX_ABS_VREL = 20.0
CORNER_OBJECT_430_MAX_ABS_YVREL = 3.0
CORNER_OBJECT_430_VREL_ALPHA = 0.35
CORNER_OBJECT_430_YVREL_ALPHA = 0.35
CORNER_OBJECT_430_LATERAL_CELL_MSG_WEIGHT = 0.35
CORNER_OBJECT_430_LATERAL_CELL_SLOT_WEIGHT = 0.65
CORNER_OBJECT_430_YREL_OFFSET = 5.8
CORNER_OBJECT_430_YREL_SCALE = 1.1
CORNER_OBJECT_430_RIGHT_CELL_MIRROR = 7.0
CORNER_OBJECT_430_MIN_ABS_YREL = 0.8
CORNER_OBJECT_430_MAX_ABS_YREL = 4.2
CORNER_OBJECT_430_HISTORY_SIZE = 8
CORNER_OBJECT_430_MIN_HISTORY = 5
CORNER_OBJECT_430_MIN_INWARD_YREL_DELTA = 0.35
CORNER_OBJECT_430_MIN_RECENT_INWARD_YREL_DELTA = 0.05
CORNER_OBJECT_430_MIN_INWARD_RATIO = 0.65
CORNER_OBJECT_430_INWARD_CENTER_ABS_YREL = 1.55
CORNER_OBJECT_430_INWARD_KEEP_YVREL_ABS_YREL = 2.2
CORNER_OBJECT_430_EARLY_INWARD_NONCENTER_FRAMES = 2
CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL = 2.0
CORNER_OBJECT_STABLE_TRACK_ID_START = 1000
CORNER_SIDE_OBJECT_MAX_DREL = 0.2
CORNER_SIDE_OBJECT_MIN_ABS_YREL = 1.4
CORNER_SIDE_OBJECT_MAX_ABS_YREL = 4.5
# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
class CornerObjectTrackIdManager:
def __init__(self):
self.next_track_id = CORNER_OBJECT_STABLE_TRACK_ID_START
self.objects: dict[tuple[str, int], tuple[int, int]] = {}
def get_track_id(self, source: str, object_id: int, age: int) -> int:
key = (source, object_id)
previous = self.objects.get(key)
if previous is None or age < previous[1]:
track_id = self.next_track_id
self.next_track_id += 1
else:
track_id = previous[0]
self.objects[key] = (track_id, age)
return track_id
def clear_source(self, source: str):
self.objects = {key: value for key, value in self.objects.items() if key[0] != source}
def corner_object_position_valid(d_rel: float, y_rel: float) -> bool:
normal_object = 0.2 < d_rel < 180.0
clipped_side_object = (
0.0 <= d_rel <= CORNER_SIDE_OBJECT_MAX_DREL and
CORNER_SIDE_OBJECT_MIN_ABS_YREL <= abs(y_rel) <= CORNER_SIDE_OBJECT_MAX_ABS_YREL
)
return (normal_object or clipped_side_object) and abs(y_rel) < 40.0
def get_radar_can_parser(CP, radar_tracks, msg_start_addr, msg_count, radar_group4=False):
if not radar_tracks:
return None
#if Bus.radar not in DBC[CP.carFingerprint]:
# return None
print("RadarInterface: RadarTracks...")
if CP.flags & HyundaiFlags.CANFD:
CAN = CanBus(CP)
messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(msg_start_addr, msg_start_addr + msg_count)]
return CANParser('hyundai_canfd_radar_generated', messages, CAN.ACAN)
else:
messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(msg_start_addr, msg_start_addr + msg_count)]
#return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 1)
dbc_name = 'hyundai_kia_denso_front_radar_generated' if radar_group4 else 'hyundai_kia_mando_front_radar_generated'
return CANParser(dbc_name, messages, 1)
def get_corner_object_can_parser(CP, enabled):
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
return None
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_235_DBC}.dbc")
if not os.path.exists(dbc_path):
print(f"RadarInterface: missing {CORNER_OBJECT_235_DBC}.dbc, 0x235 corner radar disabled")
return None
CAN = CanBus(CP)
messages = [(f"CORNER_RADAR_235_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_235_START_ADDR, CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT)]
return CANParser(CORNER_OBJECT_235_DBC, messages, CAN.ACAN)
def get_corner_object_180_can_parser(CP, enabled):
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
return None
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_180_DBC}.dbc")
if not os.path.exists(dbc_path):
print(f"RadarInterface: missing {CORNER_OBJECT_180_DBC}.dbc, 0x180 corner radar disabled")
return None
CAN = CanBus(CP)
messages = [(f"CORNER_RADAR_180_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_180_START_ADDR, CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT)]
return CANParser(CORNER_OBJECT_180_DBC, messages, CAN.ACAN)
def get_corner_object_430_can_parser(CP, enabled):
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
return None
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_430_DBC}.dbc")
if not os.path.exists(dbc_path):
print(f"RadarInterface: missing {CORNER_OBJECT_430_DBC}.dbc, 0x430/0x440 corner radar disabled")
return None
CAN = CanBus(CP)
messages = [(f"CORNER_RADAR_430_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_430_LEFT_START_ADDR, CORNER_OBJECT_430_LEFT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)]
messages += [(f"CORNER_RADAR_430_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_430_RIGHT_START_ADDR, CORNER_OBJECT_430_RIGHT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)]
return CANParser(CORNER_OBJECT_430_DBC, messages, CAN.ACAN)
def get_radar_can_parser_scc(CP):
CAN = CanBus(CP)
if CP.flags & HyundaiFlags.CANFD:
messages = [("SCC_CONTROL", 50)]
bus = CAN.ECAN
else:
messages = [("SCC11", 50)]
bus = CAN.ECAN
print("$$$$$$$$ ECAN = ", CAN.ECAN)
bus = CAN.CAM if CP.flags & HyundaiFlags.CAMERA_SCC else bus
return CANParser(DBC[CP.carFingerprint][Bus.pt], messages, bus)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP, CP_IQ=None):
super().__init__(CP, CP_IQ or structs.IQCarParams())
self.v_ego = 0.0
self.canfd = True if CP.flags & HyundaiFlags.CANFD else False
self.radar_group1 = False
self.radar_group3 = False
self.radar_group4 = not self.canfd and bool(CP.extFlags & HyundaiExtFlags.RADAR_GROUP4.value)
if self.canfd:
if CP.extFlags & HyundaiExtFlags.RADAR_GROUP1.value:
self.radar_start_addr = RADAR_START_ADDR_CANFD1
self.radar_msg_count = RADAR_MSG_COUNT1
self.radar_group1 = True
elif CP.extFlags & HyundaiExtFlags.RADAR_GROUP3.value:
self.radar_start_addr = RADAR_START_ADDR_CANFD3
self.radar_msg_count = RADAR_MSG_COUNT3
self.radar_group3 = True
else:
self.radar_start_addr = RADAR_START_ADDR_CANFD2
self.radar_msg_count = RADAR_MSG_COUNT2
else:
self.radar_start_addr = RADAR_START_ADDR
self.radar_msg_count = RADAR_MSG_COUNT4 if self.radar_group4 else RADAR_MSG_COUNT
self.params = Params()
self.radar_tracks = self.params.get_int("EnableRadarTracks") >= 1
self.corner_object_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_235.value) and self.params.get_int("EnableCornerRadar") > 0
self.corner_object_180_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_180.value) and self.params.get_int("EnableCornerRadar") > 0
self.corner_object_430_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_430.value) and self.params.get_int("EnableCornerRadar") > 0
self.updated_tracks = set()
self.updated_scc = set()
self.updated_corner_objects = set()
self.updated_corner_objects_180 = set()
self.updated_corner_objects_430 = set()
self.corner_object_missed_updates = 0
self.corner_object_180_missed_updates = 0
self.corner_object_430_missed_updates = 0
self.corner_object_track_ids = CornerObjectTrackIdManager()
self.rcp_tracks = get_radar_can_parser(CP, self.radar_tracks, self.radar_start_addr, self.radar_msg_count, self.radar_group4)
self.rcp_corner_objects = get_corner_object_can_parser(CP, self.corner_object_tracks)
self.rcp_corner_objects_180 = get_corner_object_180_can_parser(CP, self.corner_object_180_tracks)
self.rcp_corner_objects_430 = get_corner_object_430_can_parser(CP, self.corner_object_430_tracks)
# Enabling raw radar tracks on legacy CAN disables the stock SCC11 stream on
# some Hyundai/Kia platforms. Camera-SCC cars may still use SCC11.
use_scc_parser = not (self.radar_tracks and not self.canfd and not (CP.flags & HyundaiFlags.CAMERA_SCC))
self.rcp_scc = get_radar_can_parser_scc(CP) if use_scc_parser else None
self.trigger_msg_scc = 416 if self.canfd else 0x420
self.trigger_msg_tracks = self.radar_start_addr + self.radar_msg_count - 1
self.trigger_msg_corner_objects = CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT - 1
self.trigger_msg_corner_objects_180 = CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT - 1
self.trigger_msg_corner_objects_430 = CORNER_OBJECT_430_RIGHT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE - 1
self.track_id = 0
self.corner_objects_available = self.rcp_corner_objects is not None or self.rcp_corner_objects_180 is not None or self.rcp_corner_objects_430 is not None
self.radar_off_can = CP.radarUnavailable and not self.corner_objects_available
print(
"RadarInterface: "
f"radarUnavailable={CP.radarUnavailable} radarTracks={self.radar_tracks} "
f"group4={self.radar_group4} "
f"corner235={self.rcp_corner_objects is not None} corner180={self.rcp_corner_objects_180 is not None} "
f"corner430={self.rcp_corner_objects_430 is not None} "
f"radarOffCan={self.radar_off_can}"
)
self.vRel_last = 0
self.dRel_last = 0
self.corner_object_430_prev_d_rel = {}
self.corner_object_430_prev_v_rel = {}
self.corner_object_430_prev_y_rel = {}
self.corner_object_430_prev_yv_rel = {}
self.corner_object_430_prev_code = {}
self.corner_object_430_history = {}
self.corner_object_430_noncenter_inward_frames = {}
# Initialize pts
if self.rcp_tracks is not None:
total_tracks = self.radar_msg_count * (2 if self.radar_group1 else 1)
for track_id in range(total_tracks):
t_id = track_id + 32
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
if self.rcp_scc is not None:
self.pts[SCC_TID] = structs.RadarData.RadarPoint()
self.pts[SCC_TID].trackId = SCC_TID
self.pts[SCC_TID].radarSource = "scc"
if self.rcp_corner_objects is not None:
for slot in range(CORNER_OBJECT_235_MSG_COUNT):
t_id = CORNER_OBJECT_235_TRACK_ID_OFFSET + slot
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
self.pts[t_id].radarSource = "corner235"
if self.rcp_corner_objects_180 is not None:
for slot in range(CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG):
t_id = CORNER_OBJECT_180_TRACK_ID_OFFSET + slot
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
self.pts[t_id].radarSource = "corner180"
if self.rcp_corner_objects_430 is not None:
for slot in range(CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * 2 * CORNER_OBJECT_430_SLOTS_PER_MSG):
t_id = CORNER_OBJECT_430_TRACK_ID_OFFSET + slot
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
self.frame = 0
def update(self, can_strings):
self.frame += 1
if self.radar_off_can or (self.rcp_tracks is None and self.rcp_scc is None and self.rcp_corner_objects is None and self.rcp_corner_objects_180 is None and self.rcp_corner_objects_430 is None):
return super().update(None)
if self.rcp_scc is not None:
vls_s = self.rcp_scc.update(can_strings)
self.updated_scc.update(vls_s)
track_ready = False
if self.radar_tracks and self.rcp_tracks is not None:
vls_t = self.rcp_tracks.update(can_strings)
self.updated_tracks.update(vls_t)
track_ready = self.trigger_msg_tracks in self.updated_tracks
corner_ready = False
if self.rcp_corner_objects is not None:
vls_c = self.rcp_corner_objects.update(can_strings)
self.updated_corner_objects.update(vls_c)
corner_ready = self.trigger_msg_corner_objects in self.updated_corner_objects
corner_180_ready = False
if self.rcp_corner_objects_180 is not None:
vls_180 = self.rcp_corner_objects_180.update(can_strings)
self.updated_corner_objects_180.update(vls_180)
corner_180_ready = self.trigger_msg_corner_objects_180 in self.updated_corner_objects_180
corner_430_ready = False
if self.rcp_corner_objects_430 is not None:
vls_430 = self.rcp_corner_objects_430.update(can_strings)
self.updated_corner_objects_430.update(vls_430)
corner_430_ready = self.trigger_msg_corner_objects_430 in self.updated_corner_objects_430
scc_ready = not self.radar_tracks and self.frame % 5 == 0 and self.rcp_scc is not None
if track_ready:
self._update(self.updated_tracks)
self.updated_tracks.clear()
if corner_ready:
self._update_corner_objects(self.updated_corner_objects)
self.corner_object_missed_updates = 0
self.updated_corner_objects.clear()
if corner_180_ready:
self._update_corner_objects_180(self.updated_corner_objects_180)
self.corner_object_180_missed_updates = 0
self.updated_corner_objects_180.clear()
if corner_430_ready:
self._update_corner_objects_430(self.updated_corner_objects_430)
self.corner_object_430_missed_updates = 0
self.updated_corner_objects_430.clear()
# Corner radar runs at its own cadence. Do not let corner-only frames publish
# RadarData, since liveTracks uses a fixed radarTimeStep for aLead/jLead.
publish_ready = track_ready or scc_ready
if not publish_ready:
return None
if self.rcp_scc is not None:
self._update_scc(self.updated_scc)
if self.rcp_corner_objects is not None:
if self.updated_corner_objects:
self._update_corner_objects(self.updated_corner_objects)
self.corner_object_missed_updates = 0
else:
self.corner_object_missed_updates += 1
if self.corner_object_missed_updates > 10:
self._clear_corner_objects()
if self.rcp_corner_objects_180 is not None:
if self.updated_corner_objects_180:
self._update_corner_objects_180(self.updated_corner_objects_180)
self.corner_object_180_missed_updates = 0
else:
self.corner_object_180_missed_updates += 1
if self.corner_object_180_missed_updates > 10:
self._clear_corner_objects_180()
if self.rcp_corner_objects_430 is not None:
if self.updated_corner_objects_430:
self._update_corner_objects_430(self.updated_corner_objects_430)
self.corner_object_430_missed_updates = 0
else:
self.corner_object_430_missed_updates += 1
if self.corner_object_430_missed_updates > 10:
self._clear_corner_objects_430()
self.updated_scc.clear()
self.updated_corner_objects.clear()
self.updated_corner_objects_180.clear()
self.updated_corner_objects_430.clear()
ret = structs.RadarData()
if ((self.rcp_tracks is not None and self.radar_tracks and not self.rcp_tracks.can_valid) or
(self.rcp_scc is not None and not self.corner_objects_available and not self.rcp_scc.can_valid) or
(self.rcp_corner_objects is not None and not self.rcp_corner_objects.can_valid) or
(self.rcp_corner_objects_180 is not None and not self.rcp_corner_objects_180.can_valid) or
(self.rcp_corner_objects_430 is not None and not self.rcp_corner_objects_430.can_valid)):
ret.errors.canError = True
ret.points = [point for point in self.pts.values() if point.measured]
return ret
def _update(self, updated_messages):
t_id = 32
for addr in range(self.radar_start_addr, self.radar_start_addr + self.radar_msg_count):
msg = self.rcp_tracks.vl[f"RADAR_TRACK_{addr:x}"]
if self.radar_group1:
valid = msg['VALID_CNT1'] > 10
elif self.radar_group3:
# Group 3 marks an empty object slot with LONG_DIST raw 0x7ff (204.7 m).
valid = msg['LONG_DIST'] < 204.7
elif self.canfd:
valid = msg['VALID_CNT'] > 10
elif self.radar_group4:
# EN: DNMWR006 exposes eight stable tracked-object slots at 0x500-0x507.
# Messages from 0x508 onward are distance-sorted raw detections without
# stable IDs, so they are excluded. OBJECT_STATE 3 is a confirmed track;
# empty slots use LONG_DIST raw 0xfff8 (409.55 m). Driving logs reached
# 317.80 m, so 325 m preserves every observed confirmed track while
# retaining margin from the empty-slot sentinel. Keep the +/-6 m
# ego/adjacent-lane envelope to suppress farther roadside reflections.
# KO: DNMWR006의 안정적인 추적 객체 슬롯은 0x500~0x507의 8개임.
# 0x508 이후 메시지는 고정 ID가 없는 거리순 raw detection이므로 제외함.
# OBJECT_STATE 3은 확정 추적 객체이며, 빈 슬롯은 LONG_DIST raw
# 0xfff8(409.55m)을 사용함. 주행 로그의 최대값은 317.80m였으므로
# 325m 상한으로 관측된 확정 트랙을 모두 보존하면서 빈 슬롯 값과 충분한
# 여유를 확보함. 원거리 도로변 반사를 줄이기 위해 좌우 6m 범위를 유지함.
valid = (msg['OBJECT_STATE'] == 3 and 0.2 < msg['LONG_DIST'] < RADAR_GROUP4_MAX_LONG_DIST and
abs(msg['LAT_DIST']) <= RADAR_GROUP4_MAX_YREL)
else:
valid = msg['STATE'] in (3, 4)
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
elif self.radar_group1:
self.pts[t_id].dRel = msg['LONG_DIST1']
self.pts[t_id].yRel = msg['LAT_DIST1']
self.pts[t_id].vRel = msg['REL_SPEED1']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL1']
self.pts[t_id].yvRel = msg['LAT_SPEED1']
elif self.canfd:
if self.radar_group3:
# Group 3 reports the object's center. Convert it to the rear surface to match SCC/vision dRel.
self.pts[t_id].dRel = max(0.0, msg['LONG_DIST'] - msg['OBJECT_LENGTH'] * 0.5 - 0.1)
else:
self.pts[t_id].dRel = msg['LONG_DIST']
self.pts[t_id].yRel = msg['LAT_DIST']
self.pts[t_id].vRel = msg['REL_SPEED']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan') if self.radar_group3 else msg['REL_ACCEL']
self.pts[t_id].yvRel = 0.0 if self.radar_group3 else msg['LAT_SPEED']
elif self.radar_group4:
self.pts[t_id].dRel = msg['LONG_DIST']
self.pts[t_id].yRel = -msg['LAT_DIST']
self.pts[t_id].vRel = msg['REL_SPEED']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0.0
else:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[t_id].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[t_id].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[t_id].vRel = msg['REL_SPEED']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL']
self.pts[t_id].yvRel = 0.0
t_id += 1
# radar group1? ?나??msg??2개의 ?이?? ?어?음.
if self.radar_group1:
for addr in range(self.radar_start_addr, self.radar_start_addr + self.radar_msg_count):
msg = self.rcp_tracks.vl[f"RADAR_TRACK_{addr:x}"]
valid = msg['VALID_CNT2'] > 10
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = msg['LONG_DIST2']
self.pts[t_id].yRel = msg['LAT_DIST2']
self.pts[t_id].vRel = msg['REL_SPEED2']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL2']
self.pts[t_id].yvRel = msg['LAT_SPEED2']
t_id += 1
def _update_corner_objects(self, updated_messages):
if self.rcp_corner_objects is None:
return
if not updated_messages:
self._clear_corner_objects()
return
candidates = []
for slot, addr in enumerate(range(CORNER_OBJECT_235_START_ADDR, CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT)):
t_id = CORNER_OBJECT_235_TRACK_ID_OFFSET + slot
msg = self.rcp_corner_objects.vl[f"CORNER_RADAR_235_OBJECTS_{addr:x}"]
d_rel = msg["OBJ_REL_POS_X"]
y_rel = msg["OBJ_REL_POS_Y"]
v_rel = msg["OBJ_REL_VEL_X"]
yv_rel = msg["OBJ_REL_VEL_Y"]
a_rel = msg["OBJ_REL_ACCEL_X"]
# Side objects are clipped to x=0 by the corner radar. Quality, identity,
# and lateral motion still describe a real object, so keep them for
# corner-confirmed front-radar association in radard.
valid = msg["OBJ_QUAL_LEVEL"] > 0 and corner_object_position_valid(d_rel, y_rel) and v_rel > -99.0
if not valid:
continue
candidates.append((t_id, int(msg["OBJ_OBJECT_ID"]), int(msg["OBJ_AGE"]), int(msg["OBJ_QUAL_LEVEL"]),
d_rel, y_rel, v_rel, yv_rel, a_rel))
self._apply_corner_objects("corner235", candidates,
range(CORNER_OBJECT_235_TRACK_ID_OFFSET,
CORNER_OBJECT_235_TRACK_ID_OFFSET + CORNER_OBJECT_235_MSG_COUNT))
def _update_corner_objects_180(self, updated_messages):
if self.rcp_corner_objects_180 is None:
return
if not updated_messages:
self._clear_corner_objects_180()
return
candidates = []
for msg_index, addr in enumerate(range(CORNER_OBJECT_180_START_ADDR, CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT)):
msg = self.rcp_corner_objects_180.vl[f"CORNER_RADAR_180_OBJECTS_{addr:x}"]
for slot_index in range(CORNER_OBJECT_180_SLOTS_PER_MSG):
t_id = CORNER_OBJECT_180_TRACK_ID_OFFSET + msg_index * CORNER_OBJECT_180_SLOTS_PER_MSG + slot_index
prefix = f"SLOT{slot_index + 1}_"
d_rel = msg[f"{prefix}REL_POS_X"]
y_rel = msg[f"{prefix}REL_POS_Y"]
v_rel = msg[f"{prefix}REL_VEL_X"]
yv_rel = msg[f"{prefix}REL_VEL_Y"]
a_rel = msg[f"{prefix}REL_ACCEL_X"]
valid = msg[f"{prefix}QUAL_LEVEL"] > 0 and corner_object_position_valid(d_rel, y_rel) and v_rel > -99.0
if not valid:
continue
candidates.append((t_id, int(msg[f"{prefix}OBJECT_ID"]), int(msg[f"{prefix}AGE"]), int(msg[f"{prefix}QUAL_LEVEL"]),
d_rel, y_rel, v_rel, yv_rel, a_rel))
self._apply_corner_objects("corner180", candidates,
range(CORNER_OBJECT_180_TRACK_ID_OFFSET,
CORNER_OBJECT_180_TRACK_ID_OFFSET + CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG))
def _apply_corner_objects(self, source, candidates, slot_ids):
for t_id in slot_ids:
self._clear_point(t_id)
# The same object can occupy two CAN slots for one cycle during a slot handoff.
# Publish only the newest/highest-quality copy so trackId stays unique.
objects = {}
for candidate in candidates:
object_id = candidate[1]
previous = objects.get(object_id)
if previous is None or (candidate[2], candidate[3]) > (previous[2], previous[3]):
objects[object_id] = candidate
for t_id, object_id, age, _, d_rel, y_rel, v_rel, yv_rel, a_rel in objects.values():
point = self.pts[t_id]
point.measured = True
point.trackId = self.corner_object_track_ids.get_track_id(source, object_id, age)
point.radarSource = source
point.dRel = d_rel
point.yRel = y_rel
point.vRel = v_rel
point.vLead = v_rel + self.v_ego
point.aRel = a_rel
point.yvRel = yv_rel
def _update_corner_objects_430(self, updated_messages):
if self.rcp_corner_objects_430 is None:
return
if not updated_messages:
self._clear_corner_objects_430()
return
bank_defs = (
(CORNER_OBJECT_430_LEFT_START_ADDR, 1.0, 0),
(CORNER_OBJECT_430_RIGHT_START_ADDR, -1.0, CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * CORNER_OBJECT_430_SLOTS_PER_MSG),
)
for start_addr, side_sign, track_base in bank_defs:
bins = []
for msg_index, addr in enumerate(range(start_addr, start_addr + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)):
msg = self.rcp_corner_objects_430.vl[f"CORNER_RADAR_430_OBJECTS_{addr:x}"]
for slot_index in range(CORNER_OBJECT_430_SLOTS_PER_MSG):
prefix = f"SLOT{slot_index + 1}_"
distance_raw = int(msg[f"{prefix}DISTANCE_RAW"])
raw = (
distance_raw |
(int(msg[f"{prefix}META_13_15"]) << 13) |
(int(msg[f"{prefix}META_BYTE_2"]) << 16) |
(int(msg[f"{prefix}META_BYTE_3"]) << 24)
)
code = (
int(msg[f"{prefix}META_13_15"]),
int(msg[f"{prefix}META_BYTE_2"]),
int(msg[f"{prefix}META_BYTE_3"]),
)
d_rel = distance_raw * 0.05
default_distance = CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MIN <= distance_raw <= CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MAX
base_valid = (
raw not in CORNER_OBJECT_430_EMPTY_RAW_VALUES and
distance_raw not in (0, 8000, 8191) and
not default_distance and
0.2 < d_rel < CORNER_OBJECT_430_MAX_DREL
)
candidate_valid = (
base_valid and
slot_index + 1 not in CORNER_OBJECT_430_CANDIDATE_EXCLUDED_SLOTS and
code[2] in CORNER_OBJECT_430_CANDIDATE_META_BYTE_3 and
code[1] in CORNER_OBJECT_430_STRONG_META_BYTE_2 + CORNER_OBJECT_430_WEAK_META_BYTE_2
)
bins.append({
"msg_index": msg_index,
"slot_index": slot_index,
"distance_raw": distance_raw,
"d_rel": d_rel,
"code": code,
"candidate_valid": candidate_valid,
})
supported_bins = []
candidates = [b for b in bins if b["candidate_valid"]]
for b in candidates:
support = 1
for other in candidates:
if other is b:
continue
if abs(other["msg_index"] - b["msg_index"]) > 1:
continue
if abs(other["slot_index"] - b["slot_index"]) > 2:
continue
if abs(other["distance_raw"] - b["distance_raw"]) > CORNER_OBJECT_430_CANDIDATE_RAW_DELTA:
continue
support += 1
min_support = (CORNER_OBJECT_430_STRONG_MIN_SUPPORT if b["code"][1] in CORNER_OBJECT_430_STRONG_META_BYTE_2
else CORNER_OBJECT_430_WEAK_MIN_SUPPORT)
if support >= min_support:
supported_bins.append({**b, "support": support})
clusters = []
for b in sorted(supported_bins, key=lambda item: item["distance_raw"]):
if not clusters or b["distance_raw"] - clusters[-1][-1]["distance_raw"] > CORNER_OBJECT_430_CLUSTER_RAW_GAP:
clusters.append([b])
else:
clusters[-1].append(b)
clusters = sorted(clusters, key=lambda cluster: sum(b["distance_raw"] for b in cluster) / len(cluster))[:CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE]
cluster_objects = []
for cluster in clusters:
msg_index = sum(b["msg_index"] for b in cluster) / len(cluster)
slot = sum(b["slot_index"] + 1 for b in cluster) / len(cluster)
lateral_cell = (CORNER_OBJECT_430_LATERAL_CELL_MSG_WEIGHT * msg_index +
CORNER_OBJECT_430_LATERAL_CELL_SLOT_WEIGHT * slot)
mapped_cell = lateral_cell if side_sign > 0.0 else CORNER_OBJECT_430_RIGHT_CELL_MIRROR - lateral_cell
y_abs = max(CORNER_OBJECT_430_MIN_ABS_YREL,
min(CORNER_OBJECT_430_MAX_ABS_YREL,
CORNER_OBJECT_430_YREL_OFFSET - CORNER_OBJECT_430_YREL_SCALE * mapped_cell))
cluster_objects.append({
"d_rel": sum(b["d_rel"] for b in cluster) / len(cluster),
"y_rel": side_sign * y_abs,
"code": max((b["code"] for b in cluster), key=lambda code: sum(1 for item in cluster if item["code"] == code)),
})
active_t_ids = set()
side_track_ids = [
CORNER_OBJECT_430_TRACK_ID_OFFSET + track_base + slot
for slot in range(CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE)
]
unmatched_track_ids = {t_id for t_id in side_track_ids if t_id in self.corner_object_430_prev_d_rel}
unused_track_ids = [t_id for t_id in side_track_ids if t_id not in unmatched_track_ids]
for cluster in cluster_objects:
d_rel = cluster["d_rel"]
code = cluster["code"]
matched_t_id = None
if unmatched_track_ids:
nearest_t_id = min(unmatched_track_ids, key=lambda t_id: abs(d_rel - self.corner_object_430_prev_d_rel[t_id]))
if abs(d_rel - self.corner_object_430_prev_d_rel[nearest_t_id]) <= CORNER_OBJECT_430_TRACK_MATCH_MAX_DREL_DELTA:
matched_t_id = nearest_t_id
unmatched_track_ids.remove(matched_t_id)
if matched_t_id is None and unused_track_ids:
matched_t_id = unused_track_ids.pop(0)
if matched_t_id is None:
continue
t_id = matched_t_id
active_t_ids.add(t_id)
prev_d_rel = self.corner_object_430_prev_d_rel.get(t_id)
prev_code = self.corner_object_430_prev_code.get(t_id)
self.corner_object_430_prev_d_rel[t_id] = d_rel
self.corner_object_430_prev_y_rel[t_id] = cluster["y_rel"]
self.corner_object_430_prev_code[t_id] = code
reset_track = prev_d_rel is None or code != prev_code or abs(d_rel - prev_d_rel) > CORNER_OBJECT_430_MAX_DREL_DELTA
if reset_track:
self.corner_object_430_prev_v_rel.pop(t_id, None)
self.corner_object_430_prev_yv_rel.pop(t_id, None)
self.corner_object_430_history.pop(t_id, None)
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
history = self.corner_object_430_history.setdefault(t_id, deque(maxlen=CORNER_OBJECT_430_HISTORY_SIZE))
history.append((d_rel, cluster["y_rel"]))
if len(history) < CORNER_OBJECT_430_MIN_HISTORY:
self._clear_point(t_id)
continue
window_dt = CORNER_OBJECT_430_DT * (len(history) - 1)
first_d_rel, first_y_rel = history[0]
hist_v_rel = (d_rel - first_d_rel) / window_dt
if abs(hist_v_rel) > CORNER_OBJECT_430_MAX_ABS_VREL:
self.corner_object_430_prev_v_rel.pop(t_id, None)
self.corner_object_430_prev_yv_rel.pop(t_id, None)
self.corner_object_430_history.pop(t_id, None)
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
self._clear_point(t_id)
continue
prev_v_rel = self.corner_object_430_prev_v_rel.get(t_id, hist_v_rel)
v_rel = (1.0 - CORNER_OBJECT_430_VREL_ALPHA) * prev_v_rel + CORNER_OBJECT_430_VREL_ALPHA * hist_v_rel
self.corner_object_430_prev_v_rel[t_id] = v_rel
inward_steps = 0
usable_steps = 0
prev_abs_y = abs(history[0][1])
for _, y_rel in list(history)[1:]:
abs_y = abs(y_rel)
delta = prev_abs_y - abs_y
if abs(delta) > 1e-3:
usable_steps += 1
if delta > 0.0:
inward_steps += 1
prev_abs_y = abs_y
net_inward_y = abs(first_y_rel) - abs(cluster["y_rel"])
inward_ratio = inward_steps / usable_steps if usable_steps > 0 else 0.0
hist_yv_rel = (cluster["y_rel"] - first_y_rel) / window_dt
recent_inward_y = abs(history[-3][1]) - abs(cluster["y_rel"]) if len(history) >= 3 else net_inward_y
if (net_inward_y < CORNER_OBJECT_430_MIN_INWARD_YREL_DELTA or
recent_inward_y < CORNER_OBJECT_430_MIN_RECENT_INWARD_YREL_DELTA or
inward_ratio < CORNER_OBJECT_430_MIN_INWARD_RATIO or
abs(hist_yv_rel) > CORNER_OBJECT_430_MAX_ABS_YVREL):
hist_yv_rel = 0.0
inward_motion_candidate = hist_yv_rel != 0.0 and abs(cluster["y_rel"]) <= CORNER_OBJECT_430_INWARD_KEEP_YVREL_ABS_YREL
inward_center_candidate = inward_motion_candidate and abs(cluster["y_rel"]) <= CORNER_OBJECT_430_INWARD_CENTER_ABS_YREL
y_rel = cluster["y_rel"]
if inward_motion_candidate:
if inward_center_candidate:
self.corner_object_430_noncenter_inward_frames[t_id] = 0
prev_yv_rel = self.corner_object_430_prev_yv_rel.get(t_id, hist_yv_rel)
yv_rel = (1.0 - CORNER_OBJECT_430_YVREL_ALPHA) * prev_yv_rel + CORNER_OBJECT_430_YVREL_ALPHA * hist_yv_rel
else:
noncenter_frames = self.corner_object_430_noncenter_inward_frames.get(t_id, 0) + 1
self.corner_object_430_noncenter_inward_frames[t_id] = noncenter_frames
if noncenter_frames <= CORNER_OBJECT_430_EARLY_INWARD_NONCENTER_FRAMES:
prev_yv_rel = self.corner_object_430_prev_yv_rel.get(t_id, hist_yv_rel)
yv_rel = (1.0 - CORNER_OBJECT_430_YVREL_ALPHA) * prev_yv_rel + CORNER_OBJECT_430_YVREL_ALPHA * hist_yv_rel
else:
yv_rel = 0.0
if not inward_center_candidate and abs(y_rel) < CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL:
y_rel = math.copysign(CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL, y_rel)
else:
hist_yv_rel = 0.0
yv_rel = 0.0
self.corner_object_430_noncenter_inward_frames[t_id] = 0
if abs(y_rel) < CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL:
y_rel = math.copysign(CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL, y_rel)
self.corner_object_430_prev_yv_rel[t_id] = yv_rel
self.pts[t_id].measured = True
self.pts[t_id].trackId = t_id
self.pts[t_id].dRel = d_rel
self.pts[t_id].yRel = y_rel
self.pts[t_id].vRel = v_rel
self.pts[t_id].vLead = v_rel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = yv_rel
side_track_count = CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * CORNER_OBJECT_430_SLOTS_PER_MSG
for slot in range(side_track_count):
t_id = CORNER_OBJECT_430_TRACK_ID_OFFSET + track_base + slot
if t_id in active_t_ids:
continue
self.corner_object_430_prev_d_rel.pop(t_id, None)
self.corner_object_430_prev_v_rel.pop(t_id, None)
self.corner_object_430_prev_y_rel.pop(t_id, None)
self.corner_object_430_prev_yv_rel.pop(t_id, None)
self.corner_object_430_prev_code.pop(t_id, None)
self.corner_object_430_history.pop(t_id, None)
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
self._clear_point(t_id)
def _clear_point(self, t_id):
self.pts[t_id].measured = False
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
def _clear_corner_objects(self):
for slot in range(CORNER_OBJECT_235_MSG_COUNT):
self._clear_point(CORNER_OBJECT_235_TRACK_ID_OFFSET + slot)
self.corner_object_track_ids.clear_source("corner235")
def _clear_corner_objects_180(self):
for slot in range(CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG):
self._clear_point(CORNER_OBJECT_180_TRACK_ID_OFFSET + slot)
self.corner_object_track_ids.clear_source("corner180")
def _clear_corner_objects_430(self):
self.corner_object_430_prev_d_rel.clear()
self.corner_object_430_prev_v_rel.clear()
self.corner_object_430_prev_y_rel.clear()
self.corner_object_430_prev_yv_rel.clear()
self.corner_object_430_prev_code.clear()
self.corner_object_430_history.clear()
self.corner_object_430_noncenter_inward_frames.clear()
for slot in range(CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * 2 * CORNER_OBJECT_430_SLOTS_PER_MSG):
self._clear_point(CORNER_OBJECT_430_TRACK_ID_OFFSET + slot)
def _update_scc(self, updated_messages):
cpt = self.rcp_scc.vl
t_id = SCC_TID
if self.canfd:
dRel = cpt["SCC_CONTROL"]['ACC_ObjDist']
vRel = cpt["SCC_CONTROL"]['ACC_ObjRelSpd']
new_pts = abs(dRel - self.dRel_last) > 3 or abs(vRel - self.vRel_last) > 1
vLead = vRel + self.v_ego
valid = 0 < dRel < 150 and not new_pts #cpt["SCC_CONTROL"]['OBJ_STATUS'] and dRel < 150
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = dRel
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = vRel
self.pts[t_id].vLead = vLead
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0 #float('nan')
else:
dRel = cpt["SCC11"]['ACC_ObjDist']
vRel = cpt["SCC11"]['ACC_ObjRelSpd']
new_pts = abs(dRel - self.dRel_last) > 3 or abs(vRel - self.vRel_last) > 1
vLead = vRel + self.v_ego
valid = cpt["SCC11"]['ACC_ObjStatus'] and dRel < 150 and not new_pts
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = dRel
self.pts[t_id].yRel = -cpt["SCC11"]['ACC_ObjLatPos'] # in car frame's y axis, left is negative
self.pts[t_id].vRel = vRel
self.pts[t_id].vLead = vLead
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0 #float('nan')
self.dRel_last = dRel
self.vRel_last = vRel
View Git Blame Copy Permalink