Revert "radard: tie radard frequency to modelV2 (#29063)" (#29243)

This reverts commit 255f5c13f2.

cereal

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

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 None
+      return super().update(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 None
+      return super().update(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 None
+      return super().update(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 None
+      return super().update(None)
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
@@ -54,32 +54,25 @@ class RadarInterface(RadarInterfaceBase):
     ret = car.RadarData.new_message()
 
     for ii in sorted(updated_messages):
-      combined_vals = self.rcp.vl_all[ii]
-      n_vals_per_addr = len(list(combined_vals.values())[0])
-      cpts = [
-        {k: v[i] for k, v in  combined_vals.items()}
-        for i in range(n_vals_per_addr)
-      ]
-
-      for cpt in cpts:
-        if ii == 0x400:
-          # check for radar faults
-          self.radar_fault = cpt['RADAR_STATE'] != 0x79
-          self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
-        elif cpt['LONG_DIST'] < 255:
-          if ii not in self.pts or cpt['NEW_TRACK']:
-            self.pts[ii] = car.RadarData.RadarPoint.new_message()
-            self.pts[ii].trackId = self.track_id
-            self.track_id += 1
-          self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
-          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]
+      cpt = self.rcp.vl[ii]
+      if ii == 0x400:
+        # check for radar faults
+        self.radar_fault = cpt['RADAR_STATE'] != 0x79
+        self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
+      elif cpt['LONG_DIST'] < 255:
+        if ii not in self.pts or cpt['NEW_TRACK']:
+          self.pts[ii] = car.RadarData.RadarPoint.new_message()
+          self.pts[ii].trackId = self.track_id
+          self.track_id += 1
+        self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
+        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 None
+      return super().update(None)
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)

selfdrive/car/interfaces.py

@@ -1,5 +1,6 @@
 import yaml
 import os
+import time
 from abc import abstractmethod, ABC
 from typing import Any, Dict, Optional, Tuple, List, Callable
 
@@ -310,9 +311,14 @@ 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):
-    pass
+    ret = car.RadarData.new_message()
+    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 None
+      return super().update(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 None
+      return super().update(None)
 
     vls = self.rcp.update_strings(can_strings)
     self.updated_messages.update(vls)
@@ -67,38 +67,33 @@ class RadarInterface(RadarInterfaceBase):
 
     for ii in sorted(updated_messages):
       if ii in self.RADAR_A_MSGS:
-        combined_vals = self.rcp.vl_all[ii]
-        n_vals_per_addr = len(list(combined_vals.values())[0])
-        cpts = [
-          {k: v[i] for k, v in  combined_vals.items()}
-          for i in range(n_vals_per_addr)
-        ]
-
-        for cpt in cpts:
-          if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
-            self.valid_cnt[ii] = 0    # reset counter
-          if cpt['VALID'] and cpt['LONG_DIST'] < 255:
-            self.valid_cnt[ii] += 1
-          else:
-            self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
-
-          score = self.rcp.vl[ii+16]['SCORE']
-
-          # radar point only valid if it's a valid measurement and score is above 50
-          if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
-            if ii not in self.pts or cpt['NEW_TRACK']:
-              self.pts[ii] = car.RadarData.RadarPoint.new_message()
-              self.pts[ii].trackId = self.track_id
-              self.track_id += 1
-            self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
-            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 = bool(cpt['VALID'])
-          else:
-            if ii in self.pts:
-              del self.pts[ii]
+        cpt = self.rcp.vl[ii]
+
+        if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
+          self.valid_cnt[ii] = 0    # reset counter
+        if cpt['VALID'] and cpt['LONG_DIST'] < 255:
+          self.valid_cnt[ii] += 1
+        else:
+          self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
+
+        score = self.rcp.vl[ii+16]['SCORE']
+        # print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
+
+        # radar point only valid if it's a valid measurement and score is above 50
+        if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
+          if ii not in self.pts or cpt['NEW_TRACK']:
+            self.pts[ii] = car.RadarData.RadarPoint.new_message()
+            self.pts[ii].trackId = self.track_id
+            self.track_id += 1
+          self.pts[ii].dRel = cpt['LONG_DIST']  # from front of car
+          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 = 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, DT_MDL
+from common.realtime import Ratekeeper, Priority, config_realtime_process
 from system.swaglog import cloudlog
 
 from common.kalman.simple_kalman import KF1D
@@ -50,8 +50,7 @@ class KalmanParams:
 
 
 class Track:
-  def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
-    self.identifier = identifier
+  def __init__(self, v_lead: float, kalman_params: KalmanParams):
     self.cnt = 0
     self.aLeadTau = _LEAD_ACCEL_TAU
     self.K_A = kalman_params.A
@@ -64,12 +63,8 @@ class Track:
     self.dRel = d_rel   # LONG_DIST
     self.yRel = y_rel   # -LAT_DIST
     self.vRel = v_rel   # REL_SPEED
-    self.measured = measured   # measured or estimate
-
-    self.update_vlead(v_lead)
-
-  def update_vlead(self, v_lead: float):
     self.vLead = v_lead
+    self.measured = measured   # measured or estimate
 
     # computed velocity and accelerations
     if self.cnt > 0:
@@ -103,12 +98,11 @@ 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,
-      "radarTrackId": self.identifier,
+      "aLeadTau": float(self.aLeadTau)
     }
 
   def potential_low_speed_lead(self, v_ego: float):
@@ -164,16 +158,14 @@ 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,
-    "radarTrackId": -1,
+    "status": True
   }
 
 
 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
-  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:
+  if len(tracks) > 0 and ready and lead_msg.prob > .5:
     track = match_vision_to_track(v_ego, lead_msg, tracks)
   else:
     track = None
@@ -181,7 +173,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 not lead_msg_empty and lead_msg.prob > .5:
+  elif (track is None) and ready and (lead_msg.prob > .5):
     lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
 
   if low_speed_override:
@@ -211,14 +203,14 @@ class RadarD:
 
     self.ready = False
 
-  def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]):
+  def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
     self.current_time = 1e-9*max(sm.logMonoTime.values())
 
     radar_points = []
     radar_errors = []
-    if radar_data is not None:
-      radar_points = radar_data.points
-      radar_errors = radar_data.errors
+    if rr is not None:
+      radar_points = rr.points
+      radar_errors = rr.errors
 
     if sm.updated['carState']:
       self.v_ego = sm['carState'].vEgo
@@ -226,32 +218,26 @@ class RadarD:
     if sm.updated['modelV2']:
       self.ready = True
 
-    if radar_data is not None:
-      ar_pts = {}
-      for pt in radar_points:
-        ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
+    ar_pts = {}
+    for pt in radar_points:
+      ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
 
-      # *** remove missing points from meta data ***
-      for ids in list(self.tracks.keys()):
-        if ids not in ar_pts:
-          self.tracks.pop(ids, None)
+    # *** remove missing points from meta data ***
+    for ids in list(self.tracks.keys()):
+      if ids not in ar_pts:
+        self.tracks.pop(ids, None)
 
-      # *** compute the tracks ***
-      for ids in ar_pts:
-        rpt = ar_pts[ids]
+    # *** compute the tracks ***
+    for ids in ar_pts:
+      rpt = ar_pts[ids]
 
-        # align v_ego by a fixed time to align it with the radar measurement
-        v_lead = rpt[2] + self.v_ego_hist[0]
+      # align v_ego by a fixed time to align it with the radar measurement
+      v_lead = rpt[2] + self.v_ego_hist[0]
 
-        # create the track if it doesn't exist or it's a new track
-        if ids not in self.tracks:
-          self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
-        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)
+      # create the track if it doesn't exist or it's a new track
+      if ids not in self.tracks:
+        self.tracks[ids] = Track(v_lead, self.kalman_params)
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
 
     # *** publish radarState ***
     self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0
@@ -304,30 +290,32 @@ 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'], poll=["modelV2"])
+    sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState'])  # Can't check average frequency, since radar determines timing
   if pm is None:
     pm = messaging.PubMaster(['radarState', 'liveTracks'])
 
-  interface = RadarInterface(CP)
+  RI = RadarInterface(CP)
 
-  rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None)
-  radar = RadarD(DT_MDL, interface.delay)
+  rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
+  RD = RadarD(CP.radarTimeStep, RI.delay)
 
-  while True:
-    sm.update()
+  while 1:
+    can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+    rr = RI.update(can_strings)
 
-    if sm.updated['modelV2']:
-      can_strings = messaging.drain_sock_raw(can_sock)
-      radar_data = interface.update(can_strings)
+    if rr is None:
+      continue
+
+    sm.update(0)
 
-      radar.update(sm, radar_data)
-      radar.publish(pm, -rk.remaining*1000.0)
+    RD.update(sm, rr)
+    RD.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("modelV2"),
-    unlocked_pubs=["can"],
+    should_recv_callback=MessageBasedRcvCallback("can"),
+    main_pub="can",
   ),
   ProcessConfig(
     proc_name="plannerd",

selfdrive/test/process_replay/ref_commit

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