diff --git a/messaging/__init__.py b/messaging/__init__.py index 3ad41d8d650ee2..1628dd52442ab7 100644 --- a/messaging/__init__.py +++ b/messaging/__init__.py @@ -191,7 +191,7 @@ def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStruct self.updated[s] = True if self.rcv_time[s] > 1e-5 and self.freq[s] > 1e-5 and (s not in self.non_polled_services) \ - and (s not in self.ignore_average_freq) and (not SIMULATION): + and (s not in self.ignore_average_freq): self.recv_dts[s].append(cur_time - self.rcv_time[s]) self.rcv_time[s] = cur_time @@ -200,19 +200,23 @@ def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStruct self.logMonoTime[s] = msg.logMonoTime self.valid[s] = msg.valid - for s in self.data: - # arbitrary small number to avoid float comparison. If freq is 0, we can skip the check - if self.freq[s] > 1e-5: - # alive if delay is within 10x the expected frequency - self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s]) - - # alive if average frequency is higher than 90% of expected frequency - avg_dt = sum(self.recv_dts[s]) / AVG_FREQ_HISTORY - expected_dt = 1 / (self.freq[s] * 0.90) - self.alive[s] = self.alive[s] and (avg_dt < expected_dt) - else: + if SIMULATION: self.alive[s] = True + if not SIMULATION: + for s in self.data: + # arbitrary small number to avoid float comparison. If freq is 0, we can skip the check + if self.freq[s] > 1e-5: + # alive if delay is within 10x the expected frequency + self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s]) + + # alive if average frequency is higher than 90% of expected frequency + avg_dt = sum(self.recv_dts[s]) / AVG_FREQ_HISTORY + expected_dt = 1 / (self.freq[s] * 0.90) + self.alive[s] = self.alive[s] and (avg_dt < expected_dt) + else: + self.alive[s] = True + def all_alive(self, service_list=None) -> bool: if service_list is None: # check all service_list = self.alive.keys() diff --git a/messaging/socketmaster.cc b/messaging/socketmaster.cc index 32c5f10f6418ef..b10133e430bd71 100644 --- a/messaging/socketmaster.cc +++ b/messaging/socketmaster.cc @@ -1,9 +1,13 @@ #include #include +#include +#include #include "services.h" #include "messaging.h" +const bool SIMULATION = (getenv("SIMULATION") != nullptr) && (std::string(getenv("SIMULATION")) == "1"); + static inline uint64_t nanos_since_boot() { struct timespec t; clock_gettime(CLOCK_BOOTTIME, &t); @@ -103,11 +107,14 @@ void SubMaster::update_msgs(uint64_t current_time, std::vectorrcv_time = current_time; m->rcv_frame = frame; m->valid = m->event.getValid(); + if (SIMULATION) m->alive = true; } - for (auto &kv : messages_) { - SubMessage *m = kv.second; - m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq)); + if (!SIMULATION) { + for (auto &kv : messages_) { + SubMessage *m = kv.second; + m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq)); + } } }