better op

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()):

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))