check avg freq

opendbc/can/common.h

@@ -49,7 +49,9 @@ class MessageState {
  std::vector<std::vector<double>> all_vals;
 
  uint64_t last_seen_nanos;
- uint64_t check_threshold;
+ double frequency = 0.0;
+ uint64_t sum_intervals = 0;
+ uint64_t message_count = 0;
 
  uint8_t counter;
  uint8_t counter_fail;
@@ -59,6 +61,8 @@ class MessageState {
 
  bool parse(uint64_t nanos, const std::vector<uint8_t> &dat);
  bool update_counter_generic(int64_t v, int cnt_size);
+ double getAverageFreq(uint64_t current_nanos) const;
+ bool isFreqBelowThreshold(uint64_t current_nanos, double threshold) const;
 };
 
 class CANParser {

opendbc/can/parser.cc

@@ -12,6 +12,10 @@
 
 #include "opendbc/can/common.h"
 
+// The allowed deviation from the expected frequency, expressed as a fraction.
+// For example, a threshold of 0.1 allows the average frequency to be up to 10% lower than the expected value.
+const double FREQ_THRESHOLD = 0.5;
+
 int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
  int64_t ret = 0;
 
@@ -33,6 +37,13 @@ int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
 
 
 bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
+ if (last_seen_nanos != 0 && nanos > last_seen_nanos) {
+ // Update the sum of intervals with the time since the last message
+ sum_intervals += (nanos - last_seen_nanos);
+ }
+ last_seen_nanos = nanos;
+ message_count++;
+
  std::vector<double> tmp_vals(parse_sigs.size());
  bool checksum_failed = false;
  bool counter_failed = false;
@@ -72,8 +83,6 @@ bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
  vals[i] = tmp_vals[i];
  all_vals[i].push_back(vals[i]);
  }
- last_seen_nanos = nanos;
-
  return true;
 }
 
@@ -91,6 +100,23 @@ bool MessageState::update_counter_generic(int64_t v, int cnt_size) {
  return counter_fail < MAX_BAD_COUNTER;
 }
 
+double MessageState::getAverageFreq(uint64_t current_nanos) const {
+ if (message_count < 2) {
+ return 0.0; // Not enough messages
+ }
+
+ // Calculate the total interval from the first message to the current time
+ double total_interval = ((current_nanos - last_seen_nanos) + sum_intervals) / 1e9;
+ // Calculate the average interval
+ double average_interval = total_interval / message_count;
+ return 1.0 / average_interval;
+}
+
+bool MessageState::isFreqBelowThreshold(uint64_t current_nanos, double threshold) const {
+ double avg_freq = getAverageFreq(current_nanos);
+ return avg_freq < (frequency * (1.0 - threshold));
+}
+
 
 CANParser::CANParser(int abus, const std::string& dbc_name, const std::vector<std::pair<uint32_t, int>> &messages)
  : bus(abus) {
@@ -113,10 +139,11 @@ CANParser::CANParser(int abus, const std::string& dbc_name, const std::vector<st
 
  // msg is not valid if a message isn't received for 10 consecutive steps
  if (frequency > 0) {
- state.check_threshold = (1000000000ULL / frequency) * 10;
+ state.frequency = frequency;
 
+ uint64_t check_threshold = (1000000000ULL / frequency) * 10;
  // bus timeout threshold should be 10x the fastest msg
- bus_timeout_threshold = std::min(bus_timeout_threshold, state.check_threshold);
+ bus_timeout_threshold = std::min(bus_timeout_threshold, check_threshold);
  }
 
  const Msg *msg = dbc->addr_to_msg.at(address);
@@ -169,8 +196,8 @@ void CANParser::update(const std::vector<CanData> &can_data, std::vector<SignalV
  last_nanos = c.nanos;
 
  UpdateCans(c);
- UpdateValid(last_nanos);
  }
+ UpdateValid(last_nanos);
  query_latest(vals, current_nanos);
 }
 
@@ -224,17 +251,19 @@ void CANParser::UpdateValid(uint64_t nanos) {
  _counters_valid = false;
  }
 
- const bool missing = state.last_seen_nanos == 0;
- const bool timed_out = (nanos - state.last_seen_nanos) > state.check_threshold;
- if (state.check_threshold > 0 && (missing || timed_out)) {
- if (show_missing && !bus_timeout) {
- if (missing) {
- LOGE_100("0x%X '%s' NOT SEEN", state.address, state.name.c_str());
- } else if (timed_out) {
- LOGE_100("0x%X '%s' TIMED OUT", state.address, state.name.c_str());
+ if (state.frequency > 0) {
+ const bool missing = state.last_seen_nanos == 0;
+ const bool timed_out = state.last_seen_nanos < nanos && state.isFreqBelowThreshold(nanos, FREQ_THRESHOLD);
+ if (missing || timed_out) {
+ if (show_missing && !bus_timeout) {
+ if (missing) {
+ LOGE_100("0x%X '%s' NOT SEEN", state.address, state.name.c_str());
+ } else if (timed_out) {
+ LOGE_100("0x%X '%s' TIMED OUT", state.address, state.name.c_str());
+ }
  }
- }
  _valid = false;
+ }
  }
  }
  can_invalid_cnt = _valid ? 0 : (can_invalid_cnt + 1);