Reapply "radard: enable avg input service frequency checks (#31404)" (#31468)

This reverts commit d2ca0aba986826f94fc76c670e104980f93820fb.
old-commit-hash: 3cd0e5d43c

selfdrive/controls/radard.py

@@ -8,7 +8,7 @@ import capnp
 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.common.realtime import DT_CTRL, Ratekeeper, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
 
 from openpilot.common.simple_kalman import KF1D
@@ -201,6 +201,7 @@ class RadarD:
 
     self.v_ego = 0.0
     self.v_ego_hist = deque([0.0], maxlen=delay+1)
+    self.last_v_ego_frame = -1
 
     self.radar_state: Optional[capnp._DynamicStructBuilder] = None
     self.radar_state_valid = False
@@ -208,6 +209,7 @@ class RadarD:
     self.ready = False
 
   def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
+    self.ready = sm.seen['modelV2']
     self.current_time = 1e-9*max(sm.logMonoTime.values())
 
     radar_points = []
@@ -216,11 +218,10 @@ class RadarD:
       radar_points = rr.points
       radar_errors = rr.errors
 
-    if sm.updated['carState']:
+    if sm.recv_frame['carState'] != self.last_v_ego_frame:
       self.v_ego = sm['carState'].vEgo
       self.v_ego_hist.append(self.v_ego)
-    if sm.updated['modelV2']:
-      self.ready = True
+      self.last_v_ego_frame = sm.recv_frame['carState']
 
     ar_pts = {}
     for pt in radar_points:
@@ -297,7 +298,7 @@ def main():
 
   # *** setup messaging
   can_sock = messaging.sub_sock('can')
-  sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState'])  # Can't check average frequency, since radar determines timing
+  sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL))
   pm = messaging.PubMaster(['radarState', 'liveTracks'])
 
   RI = RadarInterface(CP)
@@ -308,12 +309,10 @@ def main():
   while 1:
     can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
     rr = RI.update(can_strings)
-
+    sm.update(0)
     if rr is None:
       continue
 
-    sm.update(0)
-
     RD.update(sm, rr)
     RD.publish(pm, -rk.remaining*1000.0)
 

selfdrive/test/test_onroad.py

@@ -39,7 +39,7 @@ PROCS = {
   "./ui": 18.0,
   "selfdrive.locationd.paramsd": 9.0,
   "./sensord": 7.0,
-  "selfdrive.controls.radard": 4.5,
+  "selfdrive.controls.radard": 7.0,
   "selfdrive.modeld.modeld": 13.0,
   "selfdrive.modeld.dmonitoringmodeld": 8.0,
   "selfdrive.modeld.navmodeld": 1.0,