diff --git a/messaging/__init__.py b/messaging/__init__.py index 97815f1..3ea7bcd 100644 --- a/messaging/__init__.py +++ b/messaging/__init__.py @@ -154,9 +154,9 @@ def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader: class SubMaster: - def __init__(self, services: List[str], poll: Optional[List[str]] = None, + def __init__(self, services: List[str], poll: Optional[str] = None, ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None, - ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", freq: Optional[float] = None): + ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", frequency: Optional[float] = None): self.frame = -1 self.updated = {s: False for s in services} self.recv_time = {s: 0. for s in services} @@ -169,8 +169,11 @@ class SubMaster: self.valid = {} self.logMonoTime = {} + self.max_freq = {} + self.min_freq = {} + self.poller = Poller() - polled_services = set(poll if poll is not None and len(poll) else services) + polled_services = set([poll, ] if poll is not None else services) self.non_polled_services = set(services) - polled_services self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq @@ -180,7 +183,9 @@ class SubMaster: self.ignore_alive = services self.ignore_average_freq = services - self.update_freq = freq or min([SERVICE_LIST[s].frequency for s in polled_services]) + # if freq and poll aren't specified, assume the max to be conservative + assert frequency is None or poll is None, "Do not specify 'frequency' - frequency of the polled service will be used." + self.update_freq = frequency or max([SERVICE_LIST[s].frequency for s in polled_services]) for s in services: p = self.poller if s not in self.non_polled_services else None @@ -196,6 +201,19 @@ class SubMaster: self.valid[s] = True # FIXME: this should default to False freq = max(min([SERVICE_LIST[s].frequency, self.update_freq]), 1.) + if s == poll: + max_freq = freq + min_freq = freq + else: + max_freq = min(freq, self.update_freq) + if SERVICE_LIST[s].frequency >= 2*self.update_freq: + min_freq = self.update_freq + elif self.update_freq >= 2*SERVICE_LIST[s].frequency: + min_freq = freq + else: + min_freq = min(freq, freq / 2.) + self.max_freq[s] = max_freq*1.2 + self.min_freq[s] = min_freq*0.8 self.recv_dts[s] = deque(maxlen=int(5*freq)) def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader: @@ -224,7 +242,6 @@ class SubMaster: s = msg.which() self.updated[s] = True - if self.recv_time[s] > 1e-5: self.recv_dts[s].append(cur_time - self.recv_time[s]) self.recv_time[s] = cur_time @@ -244,7 +261,7 @@ class SubMaster: except ZeroDivisionError: avg_freq = 0 expected_freq = min(SERVICE_LIST[s].frequency, self.update_freq) - self.freq_ok[s] = (len(self.recv_dts[s]) >= 2*expected_freq) and (avg_freq > expected_freq*0.8) and (avg_freq < expected_freq*1.2) + self.freq_ok[s] = (len(self.recv_dts[s]) >= 2*expected_freq) and (avg_freq > self.min_freq[s]) and (avg_freq < self.max_freq[s]) else: self.freq_ok[s] = True self.alive[s] = True diff --git a/messaging/tests/test_pub_sub_master.py b/messaging/tests/test_pub_sub_master.py index 2382c8e..81a1cf2 100755 --- a/messaging/tests/test_pub_sub_master.py +++ b/messaging/tests/test_pub_sub_master.py @@ -74,6 +74,28 @@ class TestSubMaster(unittest.TestCase): self.assertLess(t, 5) self.assertFalse(any(sm.updated.values())) + def test_avg_frequency_checks(self): + for poll in (True, False): + sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "liveCalibration"], + poll=("modelV2" if poll else None), + frequency=(20. if not poll else None)) + + checks = { + "carState": (20, 20), + "modelV2": (20, 20 if poll else 10), + "cameraOdometry": (20, 10), + "liveCalibration": (4, 4), + "carParams": (None, None), + } + + for service, (max_freq, min_freq) in checks.items(): + if max_freq is not None: + assert sm._check_avg_freq(service) + assert sm.max_freq[service] == max_freq*1.2 + assert sm.min_freq[service] == min_freq*0.8 + else: + assert not sm._check_avg_freq(service) + def test_alive(self): pass