From f1c222771951b2aa1471789560e3d22b733301c1 Mon Sep 17 00:00:00 2001 From: Eran Date: Fri, 25 Oct 2024 11:19:05 -0600 Subject: [PATCH] hkg can-fd hda1 CameraSCC radar parsing typo fix Fix radar_interface.py Fix radar_interface.py again CAN-FD hda1 radar parse.py typo fix Update radard.py another fix Update radar_interface.py Another Fix Update radard.py Yet Another fix Update radard.py --- selfdrive/car/hyundai/interface.py | 1 + selfdrive/car/hyundai/radar_interface.py | 82 +++++++++++++++++------- selfdrive/controls/radard.py | 20 +++--- 3 files changed, 72 insertions(+), 31 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index ebd8ce39981137..d9fcb075677059 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -60,6 +60,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value if candidate not in CANFD_RADAR_SCC_CAR: ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value + ret.radarUnavailable = False else: # TODO: detect EV and hybrid if candidate in HYBRID_CAR: diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 52600509860f5a..e7364da2606f8a 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -3,7 +3,8 @@ from cereal import car from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import RadarInterfaceBase -from openpilot.selfdrive.car.hyundai.values import DBC +from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus +from openpilot.selfdrive.car.hyundai.values import DBC, HyundaiFlags, CANFD_CAR RADAR_START_ADDR = 0x500 RADAR_MSG_COUNT = 32 @@ -12,7 +13,15 @@ def get_radar_can_parser(CP): if DBC[CP.carFingerprint]['radar'] is None: - return None + if CP.carFingerprint in CANFD_CAR: + if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC: + lead_src, bus = "SCC_CONTROL", CanBus(CP).CAM + else: + return None + else: + return None + messages = [(lead_src, 50)] + return CANParser(DBC[CP.carFingerprint]['pt'], messages, bus) messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT)] return CANParser(DBC[CP.carFingerprint]['radar'], messages, 1) @@ -21,8 +30,10 @@ def get_radar_can_parser(CP): class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) + self.CP = CP + self.camera_scc = CP.flags & HyundaiFlags.CANFD_CAMERA_SCC self.updated_messages = set() - self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1 + self.trigger_msg = 0x1A0 if self.camera_scc else (RADAR_START_ADDR + RADAR_MSG_COUNT - 1) self.track_id = 0 self.radar_off_can = CP.radarUnavailable @@ -41,6 +52,10 @@ def update(self, can_strings): rr = self._update(self.updated_messages) self.updated_messages.clear() + radar_error = [] + if rr is not None: + radar_error = rr.errors + return rr def _update(self, updated_messages): @@ -54,26 +69,47 @@ def _update(self, updated_messages): errors.append("canError") ret.errors = errors - for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): - msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"] - - if addr not in self.pts: - self.pts[addr] = car.RadarData.RadarPoint.new_message() - self.pts[addr].trackId = self.track_id - self.track_id += 1 - - valid = msg['STATE'] in (3, 4) - if valid: - azimuth = math.radians(msg['AZIMUTH']) - self.pts[addr].measured = True - self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST'] - self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST'] - self.pts[addr].vRel = msg['REL_SPEED'] - self.pts[addr].aRel = msg['REL_ACCEL'] - self.pts[addr].yvRel = float('nan') - - else: - del self.pts[addr] + if self.camera_scc: + msg_src = "SCC_CONTROL" + msg = self.rcp.vl[msg_src] + valid = msg['ACC_ObjDist'] < 204.6 + for ii in range(1): + if valid: + if ii not in self.pts: + self.pts[ii] = car.RadarData.RadarPoint.new_message() + self.pts[ii].trackId = self.track_id + self.track_id += 1 + self.pts[ii].measured = True + self.pts[ii].dRel = msg['ACC_ObjDist'] + self.pts[ii].yRel = float('nan') + self.pts[ii].vRel = msg['ACC_ObjRelSpd'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + + else: + if ii in self.pts: + del self.pts[ii] + else: + for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): + msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"] + + if addr not in self.pts: + self.pts[addr] = car.RadarData.RadarPoint.new_message() + self.pts[addr].trackId = self.track_id + self.track_id += 1 + + valid = msg['STATE'] in (3, 4) + if valid: + azimuth = math.radians(msg['AZIMUTH']) + self.pts[addr].measured = True + self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST'] + self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST'] + self.pts[addr].vRel = msg['REL_SPEED'] + self.pts[addr].aRel = msg['REL_ACCEL'] + self.pts[addr].yvRel = float('nan') + + else: + del self.pts[addr] ret.points = list(self.pts.values()) return ret diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index eeec3a9cdffde2..cf72277d24d385 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -12,6 +12,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.simple_kalman import KF1D +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags from openpilot.selfdrive.frogpilot.frogpilot_variables import FrogPilotVariables @@ -92,10 +93,11 @@ def reset_a_lead(self, aLeadK: float, aLeadTau: float): self.aLeadK = aLeadK self.aLeadTau = aLeadTau - def get_RadarState(self, model_prob: float = 0.0): + def get_RadarState(self, CP: car.CarParams = None, lead_msg_y: float = 0.0, model_prob: float = 0.0): + y_rel_vision = False if CP is None or CP.carName != "hyundai" else CP.flags & HyundaiFlags.CANFD_CAMERA_SCC return { "dRel": float(self.dRel), - "yRel": float(self.yRel), + "yRel": float(-lead_msg_y) if y_rel_vision else float(self.yRel), "vRel": float(self.vRel), "vLead": float(self.vLead), "vLeadK": float(self.vLeadK), @@ -181,7 +183,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, lead_detection_threshold: float, low_speed_override: bool = True) -> dict[str, Any]: + model_v_ego: float, CP: car.CarParams, lead_detection_threshold: float, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_threshold: track = match_vision_to_track(v_ego, lead_msg, tracks) @@ -190,7 +192,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = {'status': False} if track is not None: - lead_dict = track.get_RadarState(lead_msg.prob) + lead_dict = track.get_RadarState(CP, lead_msg.y[0], lead_msg.prob) elif (track is None) and ready and (lead_msg.prob > lead_detection_threshold): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) @@ -218,7 +220,7 @@ def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStruct class RadarD: - def __init__(self, radar_ts: float, delay: int = 0): + def __init__(self, radar_ts: float, CP: car.CarParams, delay: int = 0): self.points: dict[int, tuple[float, float, float]] = {} self.current_time = 0.0 @@ -236,6 +238,8 @@ def __init__(self, radar_ts: float, delay: int = 0): self.ready = False + self.CP = CP + # FrogPilot variables self.frogpilot_toggles = FrogPilotVariables.toggles FrogPilotVariables.update_frogpilot_params() @@ -295,8 +299,8 @@ def update(self, sm: messaging.SubMaster, rr): model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.frogpilot_toggles.lead_detection_threshold, low_speed_override=False) if self.frogpilot_toggles.adjacent_lead_tracking_ui: self.radar_state.leadLeft = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True) @@ -378,7 +382,7 @@ def main(): RI = RadarInterface(CP) rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) - RD = RadarD(CP.radarTimeStep, RI.delay) + RD = RadarD(CP.radarTimeStep, CP, RI.delay) # FrogPilot variables frogpilot_toggles = FrogPilotVariables.toggles