From d511d042bd18e00f6922684eec9032822fba38b2 Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 17 Jul 2026 13:02:10 -0400 Subject: [PATCH] better op --- selfdrive/controls/radard.py | 16 ++++++++++++-- selfdrive/controls/tests/test_radard.py | 29 +++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 2 deletions(-) create mode 100644 selfdrive/controls/tests/test_radard.py diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index d51eb6edb0..4bdeca6b49 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -25,6 +25,8 @@ SPEED, ACCEL = 0, 1 # Kalman filter states enum # stationary qualification parameters V_EGO_STATIONARY = 4. # no stationary object flag below this speed +DBC_MOTION_STATIONARY = 1 +DBC_MOTION_MOVING = 2 RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame @@ -197,8 +199,14 @@ def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead_dict return lead_dict +def radar_point_eligible_for_fusion(CP: structs.CarParams, CP_SP: structs.CarParamsSP, + point: car.RadarData.RadarPoint) -> bool: + use_dbc_motion = CP.brand == "hyundai" and CP_SP.flags & HyundaiFlagsSP.RADAR_FULL_RADAR + return not use_dbc_motion or point.motionState in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING) + + class RadarD: - def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams, delay: float = 0.0): + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, delay: float = 0.0): self.CP = CP self.CP_SP = CP_SP @@ -226,7 +234,11 @@ class RadarD: self.v_ego_hist.append(self.v_ego) self.last_v_ego_frame = sm.recv_frame['carState'] - ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] for pt in rr.points} + ar_pts = { + pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] + for pt in rr.points + if radar_point_eligible_for_fusion(self.CP, self.CP_SP, pt) + } # *** remove missing points from meta data *** for ids in list(self.tracks.keys()): diff --git a/selfdrive/controls/tests/test_radard.py b/selfdrive/controls/tests/test_radard.py new file mode 100644 index 0000000000..23359bbf3a --- /dev/null +++ b/selfdrive/controls/tests/test_radard.py @@ -0,0 +1,29 @@ +from cereal import car, custom + +from opendbc.car import structs +from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP +from openpilot.selfdrive.controls.radard import radar_point_eligible_for_fusion + + +def radar_point(motion_state: int): + point = car.RadarData.RadarPoint.new_message() + point.motionState = motion_state + return point + + +def test_hyundai_full_radar_fuses_only_classified_points(): + CP = structs.CarParams(brand="hyundai") + CP_SP = custom.CarParamsSP.new_message() + CP_SP.flags = HyundaiFlagsSP.RADAR_FULL_RADAR.value + + assert radar_point_eligible_for_fusion(CP, CP_SP, radar_point(1)) + assert radar_point_eligible_for_fusion(CP, CP_SP, radar_point(2)) + assert not radar_point_eligible_for_fusion(CP, CP_SP, radar_point(0)) + assert not radar_point_eligible_for_fusion(CP, CP_SP, radar_point(255)) + + +def test_radar_motion_filter_does_not_affect_other_modes_or_brands(): + CP_SP = custom.CarParamsSP.new_message() + + assert radar_point_eligible_for_fusion(structs.CarParams(brand="hyundai"), CP_SP, radar_point(0)) + assert radar_point_eligible_for_fusion(structs.CarParams(brand="toyota"), CP_SP, radar_point(0))