diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 4aeb12018a..f95baeb336 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -121,6 +121,9 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6) ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42) # Spec ret.tireStiffnessFactor = 0.385 + if candidate == CAR.KONA_EV_2022: + ret.spFlags |= HyundaiFlagsSP.SP_CAMERA_SCC_LEAD.value + ret.radarUnavailable = False elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV): ret.mass = 1490. # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx ret.wheelbase = 2.7 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 05d368b817..9c0824b185 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -13,6 +13,8 @@ def get_radar_can_parser(CP): if DBC[CP.carFingerprint]['radar'] is None: if CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC: lead_src, bus = "ESCC", 0 + elif CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD: + lead_src, bus = "SCC11", 2 else: return None messages = [(lead_src, 50)] @@ -26,8 +28,11 @@ class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) self.enhanced_scc = (CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC) and DBC[CP.carFingerprint]['radar'] is None + self.camera_scc = CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD self.updated_messages = set() - self.trigger_msg = 0x2AB if self.enhanced_scc else RADAR_START_ADDR + RADAR_MSG_COUNT - 1 + self.trigger_msg = 0x2AB if self.enhanced_scc else \ + 0x420 if self.camera_scc else \ + (RADAR_START_ADDR + RADAR_MSG_COUNT - 1) self.track_id = 0 self.radar_off_can = CP.radarUnavailable @@ -59,8 +64,9 @@ class RadarInterface(RadarInterfaceBase): errors.append("canError") ret.errors = errors - if self.enhanced_scc: - msg = self.rcp.vl["ESCC"] + if self.enhanced_scc or self.camera_scc: + msg_src = "ESCC" if self.enhanced_scc else "SCC11" + msg = self.rcp.vl[msg_src] valid = msg['ACC_ObjStatus'] for ii in range(1): if valid: @@ -70,7 +76,7 @@ class RadarInterface(RadarInterfaceBase): self.track_id += 1 self.pts[ii].measured = True self.pts[ii].dRel = msg['ACC_ObjDist'] - self.pts[ii].yRel = -msg['ACC_ObjLatPos'] + self.pts[ii].yRel = -msg['ACC_ObjLatPos'] if self.enhanced_scc else float('nan') self.pts[ii].vRel = msg['ACC_ObjRelSpd'] self.pts[ii].aRel = float('nan') self.pts[ii].yvRel = float('nan') diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 9c9cc4e0b5..fa0510ac5a 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -71,6 +71,7 @@ class HyundaiFlagsSP(IntFlag): SP_ENHANCED_SCC = 1 SP_CAN_LFA_BTN = 2 SP_NAV_MSG = 4 + SP_CAMERA_SCC_LEAD = 8 class CAR: diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index ec42ae66f2..8be4a14f5d 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -9,6 +9,7 @@ from cereal import messaging, log, car from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper, Priority, config_realtime_process +from openpilot.selfdrive.car.hyundai.values import HyundaiFlagsSP from openpilot.system.swaglog import cloudlog from openpilot.common.kalman.simple_kalman import KF1D @@ -91,10 +92,11 @@ class Track: 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 else CP.spFlags & HyundaiFlagsSP.SP_CAMERA_SCC_LEAD 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), @@ -167,7 +169,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, low_speed_override: bool = True) -> Dict[str, Any]: + model_v_ego: float, CP: car.CarParams, 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 > .5: track = match_vision_to_track(v_ego, lead_msg, tracks) @@ -176,7 +178,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 > .5): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) @@ -193,7 +195,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn class RadarD: - def __init__(self, radar_ts: float, delay: int = 0): + def __init__(self, radar_ts: float, CP: car.CarParams, delay: int = 0): self.current_time = 0.0 self.tracks: Dict[int, Track] = {} @@ -207,6 +209,8 @@ class RadarD: self.ready = False + self.CP = CP + def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]): self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -256,8 +260,8 @@ class RadarD: 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, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, 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, 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, low_speed_override=False) def publish(self, pm: messaging.PubMaster, lag_ms: float): assert self.radar_state is not None @@ -305,7 +309,7 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi 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) while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)