diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index f5f76ec2f0..b0b9350dec 100755 --- a/selfdrive/controls/radard.py +++ b/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) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index d5350239fe..c9064df870 100755 --- a/selfdrive/test/test_onroad.py +++ b/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,