radard: tie radard frequency to modelV2 (#29063)

cereal

@@ -1 +1 @@
-Subproject commit 2689300b4aba2a8850a0a8d985f39e2e3954eae9
+Subproject commit 774bf79f4da2b72d01c34a2f2d6ac9388293fbdb

selfdrive/car/chrysler/radar_interface.py

@@ -52,7 +52,7 @@ class RadarInterface(RadarInterfaceBase):
 
   def update(self, can_strings):
     if self.rcp is None or self.CP.radarUnavailable:
-      return super().update(None)
+      return None
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)

selfdrive/car/ford/radar_interface.py

@@ -62,7 +62,7 @@ class RadarInterface(RadarInterfaceBase):
 
   def update(self, can_strings):
     if self.rcp is None:
-      return super().update(None)
+      return None
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)

selfdrive/car/gm/radar_interface.py

@@ -44,7 +44,7 @@ class RadarInterface(RadarInterfaceBase):
 
   def update(self, can_strings):
     if self.rcp is None:
-      return super().update(None)
+      return None
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)

selfdrive/car/honda/radar_interface.py

@@ -38,7 +38,7 @@ class RadarInterface(RadarInterfaceBase):
     # in Bosch radar and we are only steering for now, so sleep 0.05s to keep
     # radard at 20Hz and return no points
     if self.radar_off_can:
-      return super().update(None)
+      return None
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
@@ -54,25 +54,32 @@ class RadarInterface(RadarInterfaceBase):
     ret = car.RadarData.new_message()
 
     for ii in sorted(updated_messages):
-      cpt = self.rcp.vl[ii]
+      combined_vals = self.rcp.vl_all[ii]
-      if ii == 0x400:
+      n_vals_per_addr = len(list(combined_vals.values())[0])
-        # check for radar faults
+      cpts = [
-        self.radar_fault = cpt['RADAR_STATE'] != 0x79
+        {k: v[i] for k, v in  combined_vals.items()}
-        self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
+        for i in range(n_vals_per_addr)
-      elif cpt['LONG_DIST'] < 255:
+      ]
-        if ii not in self.pts or cpt['NEW_TRACK']:
+
-          self.pts[ii] = car.RadarData.RadarPoint.new_message()
+      for cpt in cpts:
-          self.pts[ii].trackId = self.track_id
+        if ii == 0x400:
-          self.track_id += 1
+          # check for radar faults
-        self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
+          self.radar_fault = cpt['RADAR_STATE'] != 0x79
-        self.pts[ii].yRel = -cpt['LAT_DIST']  # in car frame's y axis, left is positive
+          self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
-        self.pts[ii].vRel = cpt['REL_SPEED']
+        elif cpt['LONG_DIST'] < 255:
-        self.pts[ii].aRel = float('nan')
+          if ii not in self.pts or cpt['NEW_TRACK']:
-        self.pts[ii].yvRel = float('nan')
+            self.pts[ii] = car.RadarData.RadarPoint.new_message()
-        self.pts[ii].measured = True
+            self.pts[ii].trackId = self.track_id
-      else:
+            self.track_id += 1
-        if ii in self.pts:
+          self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
-          del self.pts[ii]
+          self.pts[ii].yRel = -cpt['LAT_DIST']  # in car frame's y axis, left is positive
+          self.pts[ii].vRel = cpt['REL_SPEED']
+          self.pts[ii].aRel = float('nan')
+          self.pts[ii].yvRel = float('nan')
+          self.pts[ii].measured = True
+        else:
+          if ii in self.pts:
+            del self.pts[ii]
 
     errors = []
     if not self.rcp.can_valid:

selfdrive/car/hyundai/radar_interface.py

@@ -42,7 +42,7 @@ class RadarInterface(RadarInterfaceBase):
 
   def update(self, can_strings):
     if self.radar_off_can or (self.rcp is None):
-      return super().update(None)
+      return None
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)

selfdrive/car/interfaces.py

@@ -1,6 +1,5 @@
 import yaml
 import os
-import time
 from abc import abstractmethod, ABC
 from typing import Any, Dict, Optional, Tuple, List, Callable
 
@@ -311,14 +310,9 @@ class RadarInterfaceBase(ABC):
     self.rcp = None
     self.pts = {}
     self.delay = 0
-    self.radar_ts = CP.radarTimeStep
-    self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ
 
   def update(self, can_strings):
-    ret = car.RadarData.new_message()
+    pass
-    if not self.no_radar_sleep:
-      time.sleep(self.radar_ts)  # radard runs on RI updates
-    return ret
 
 
 class CarStateBase(ABC):

selfdrive/car/tesla/radar_interface.py

@@ -58,7 +58,7 @@ class RadarInterface(RadarInterfaceBase):
 
   def update(self, can_strings):
     if self.rcp is None:
-      return super().update(None)
+      return None
 
     values = self.rcp.update_strings(can_strings)
     self.updated_messages.update(values)

selfdrive/car/toyota/radar_interface.py

@@ -45,7 +45,7 @@ class RadarInterface(RadarInterfaceBase):
 
   def update(self, can_strings):
     if self.rcp is None:
-      return super().update(None)
+      return None
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
@@ -67,33 +67,38 @@ class RadarInterface(RadarInterfaceBase):
 
     for ii in sorted(updated_messages):
       if ii in self.RADAR_A_MSGS:
-        cpt = self.rcp.vl[ii]
+        combined_vals = self.rcp.vl_all[ii]
-
+        n_vals_per_addr = len(list(combined_vals.values())[0])
-        if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
+        cpts = [
-          self.valid_cnt[ii] = 0    # reset counter
+          {k: v[i] for k, v in  combined_vals.items()}
-        if cpt['VALID'] and cpt['LONG_DIST'] < 255:
+          for i in range(n_vals_per_addr)
-          self.valid_cnt[ii] += 1
+        ]
-        else:
+
-          self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
+        for cpt in cpts:
-
+          if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
-        score = self.rcp.vl[ii+16]['SCORE']
+            self.valid_cnt[ii] = 0    # reset counter
-        # print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
+          if cpt['VALID'] and cpt['LONG_DIST'] < 255:
-
+            self.valid_cnt[ii] += 1
-        # radar point only valid if it's a valid measurement and score is above 50
+          else:
-        if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
+            self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
-          if ii not in self.pts or cpt['NEW_TRACK']:
+
-            self.pts[ii] = car.RadarData.RadarPoint.new_message()
+          score = self.rcp.vl[ii+16]['SCORE']
-            self.pts[ii].trackId = self.track_id
+
-            self.track_id += 1
+          # radar point only valid if it's a valid measurement and score is above 50
-          self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
+          if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
-          self.pts[ii].yRel = -cpt['LAT_DIST']  # in car frame's y axis, left is positive
+            if ii not in self.pts or cpt['NEW_TRACK']:
-          self.pts[ii].vRel = cpt['REL_SPEED']
+              self.pts[ii] = car.RadarData.RadarPoint.new_message()
-          self.pts[ii].aRel = float('nan')
+              self.pts[ii].trackId = self.track_id
-          self.pts[ii].yvRel = float('nan')
+              self.track_id += 1
-          self.pts[ii].measured = bool(cpt['VALID'])
+            self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
-        else:
+            self.pts[ii].yRel = -cpt['LAT_DIST']  # in car frame's y axis, left is positive
-          if ii in self.pts:
+            self.pts[ii].vRel = cpt['REL_SPEED']
-            del self.pts[ii]
+            self.pts[ii].aRel = float('nan')
+            self.pts[ii].yvRel = float('nan')
+            self.pts[ii].measured = bool(cpt['VALID'])
+          else:
+            if ii in self.pts:
+              del self.pts[ii]
 
     ret.points = list(self.pts.values())
     return ret

selfdrive/controls/radard.py

@@ -8,7 +8,7 @@ import capnp
 from cereal import messaging, log, car
 from common.numpy_fast import interp
 from common.params import Params
-from common.realtime import Ratekeeper, Priority, config_realtime_process
+from common.realtime import Ratekeeper, Priority, config_realtime_process, DT_MDL
 from system.swaglog import cloudlog
 
 from common.kalman.simple_kalman import KF1D
@@ -50,7 +50,8 @@ class KalmanParams:
 
 
 class Track:
-  def __init__(self, v_lead: float, kalman_params: KalmanParams):
+  def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
+    self.identifier = identifier
     self.cnt = 0
     self.aLeadTau = _LEAD_ACCEL_TAU
     self.K_A = kalman_params.A
@@ -63,9 +64,13 @@ class Track:
     self.dRel = d_rel   # LONG_DIST
     self.yRel = y_rel   # -LAT_DIST
     self.vRel = v_rel   # REL_SPEED
-    self.vLead = v_lead
     self.measured = measured   # measured or estimate
 
+    self.update_vlead(v_lead)
+
+  def update_vlead(self, v_lead: float):
+    self.vLead = v_lead
+
     # computed velocity and accelerations
     if self.cnt > 0:
       self.kf.update(self.vLead)
@@ -98,11 +103,12 @@ class Track:
       "vLead": float(self.vLead),
       "vLeadK": float(self.vLeadK),
       "aLeadK": float(self.aLeadK),
+      "aLeadTau": float(self.aLeadTau),
       "status": True,
       "fcw": self.is_potential_fcw(model_prob),
       "modelProb": model_prob,
       "radar": True,
-      "aLeadTau": float(self.aLeadTau)
+      "radarTrackId": self.identifier,
     }
 
   def potential_low_speed_lead(self, v_ego: float):
@@ -158,14 +164,16 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
     "aLeadTau": 0.3,
     "fcw": False,
     "modelProb": float(lead_msg.prob),
+    "status": True,
     "radar": False,
-    "status": True
+    "radarTrackId": -1,
   }
 
 
 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]:
   # Determine leads, this is where the essential logic happens
-  if len(tracks) > 0 and ready and lead_msg.prob > .5:
+  lead_msg_empty = any([len(c) == 0 for c in [lead_msg.x, lead_msg.y, lead_msg.v]])
+  if len(tracks) > 0 and ready and not lead_msg_empty and lead_msg.prob > .5:
     track = match_vision_to_track(v_ego, lead_msg, tracks)
   else:
     track = None
@@ -173,7 +181,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)
-  elif (track is None) and ready and (lead_msg.prob > .5):
+  elif track is None and ready and not lead_msg_empty and lead_msg.prob > .5:
     lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
 
   if low_speed_override:
@@ -203,14 +211,14 @@ class RadarD:
 
     self.ready = False
 
-  def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
+  def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]):
     self.current_time = 1e-9*max(sm.logMonoTime.values())
 
     radar_points = []
     radar_errors = []
-    if rr is not None:
+    if radar_data is not None:
-      radar_points = rr.points
+      radar_points = radar_data.points
-      radar_errors = rr.errors
+      radar_errors = radar_data.errors
 
     if sm.updated['carState']:
       self.v_ego = sm['carState'].vEgo
@@ -218,26 +226,32 @@ class RadarD:
     if sm.updated['modelV2']:
       self.ready = True
 
-    ar_pts = {}
+    if radar_data is not None:
-    for pt in radar_points:
+      ar_pts = {}
-      ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
+      for pt in radar_points:
+        ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
 
-    # *** remove missing points from meta data ***
+      # *** remove missing points from meta data ***
-    for ids in list(self.tracks.keys()):
+      for ids in list(self.tracks.keys()):
-      if ids not in ar_pts:
+        if ids not in ar_pts:
-        self.tracks.pop(ids, None)
+          self.tracks.pop(ids, None)
 
-    # *** compute the tracks ***
+      # *** compute the tracks ***
-    for ids in ar_pts:
+      for ids in ar_pts:
-      rpt = ar_pts[ids]
+        rpt = ar_pts[ids]
 
-      # align v_ego by a fixed time to align it with the radar measurement
+        # align v_ego by a fixed time to align it with the radar measurement
-      v_lead = rpt[2] + self.v_ego_hist[0]
+        v_lead = rpt[2] + self.v_ego_hist[0]
 
-      # create the track if it doesn't exist or it's a new track
+        # create the track if it doesn't exist or it's a new track
-      if ids not in self.tracks:
+        if ids not in self.tracks:
-        self.tracks[ids] = Track(v_lead, self.kalman_params)
+          self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
-      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
+        self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
+    else:
+      # *** no radar points, keep existing tracks, update v_lead
+      for track in self.tracks.values():
+        v_lead = track.vRel + self.v_ego_hist[0]
+        track.update_vlead(v_lead)
 
     # *** publish radarState ***
     self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0
@@ -290,32 +304,30 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi
   cloudlog.info("radard is importing %s", CP.carName)
   RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
 
-  # *** setup messaging
+  # setup messaging
   if can_sock is None:
     can_sock = messaging.sub_sock('can')
   if sm is None:
-    sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState'])  # Can't check average frequency, since radar determines timing
+    sm = messaging.SubMaster(['modelV2', 'carState'], poll=["modelV2"])
   if pm is None:
     pm = messaging.PubMaster(['radarState', 'liveTracks'])
 
-  RI = RadarInterface(CP)
+  interface = RadarInterface(CP)
-
-  rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
-  RD = RadarD(CP.radarTimeStep, RI.delay)
 
-  while 1:
+  rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None)
-    can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+  radar = RadarD(DT_MDL, interface.delay)
-    rr = RI.update(can_strings)
 
-    if rr is None:
+  while True:
-      continue
+    sm.update()
 
-    sm.update(0)
+    if sm.updated['modelV2']:
+      can_strings = messaging.drain_sock_raw(can_sock)
+      radar_data = interface.update(can_strings)
 
-    RD.update(sm, rr)
+      radar.update(sm, radar_data)
-    RD.publish(pm, -rk.remaining*1000.0)
+      radar.publish(pm, -rk.remaining*1000.0)
 
-    rk.monitor_time()
+      rk.monitor_time()
 
 
 def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None, can_sock: messaging.SubSocket = None):

selfdrive/test/process_replay/process_replay.py

@@ -464,8 +464,8 @@ CONFIGS = [
     subs=["radarState", "liveTracks"],
     ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
     init_callback=get_car_params_callback,
-    should_recv_callback=MessageBasedRcvCallback("can"),
+    should_recv_callback=MessageBasedRcvCallback("modelV2"),
-    main_pub="can",
+    unlocked_pubs=["can"],
   ),
   ProcessConfig(
     proc_name="plannerd",

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-8fd42f02d1ecac695327bfc0afd4be1918ab680a
+4e55314f3a2502dea71d84e23d4889724c7bd05e