refactor parser

opendbc/can/common.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <map>
+#include <set>
 #include <string>
 #include <utility>
 #include <unordered_map>
@@ -45,8 +46,7 @@ class MessageState {
   unsigned int size;
 
   std::vector<Signal> parse_sigs;
-  std::vector<double> vals;
-  std::vector<std::vector<double>> all_vals;
+  std::map<std::string, SignalValue> values;
 
   uint64_t last_seen_nanos;
   uint64_t check_threshold;
@@ -79,11 +79,11 @@ class CANParser {
   CANParser(int abus, const std::string& dbc_name,
             const std::vector<std::pair<uint32_t, int>> &messages);
   CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter);
-  void update(const std::vector<CanData> &can_data, std::vector<SignalValue> &vals);
-  void query_latest(std::vector<SignalValue> &vals, uint64_t last_ts = 0);
+  MessageState *messageState(uint32_t address) { return &message_states.at(address); }
+  std::set<uint32_t> update(const std::vector<CanData> &can_data);
 
 protected:
-  void UpdateCans(const CanData &can);
+  void updateCans(const CanData &can, std::set<uint32_t> &updated_addresses);
   void UpdateValid(uint64_t nanos);
 };
 

opendbc/can/common.pxd

@@ -3,7 +3,9 @@
 
 from libc.stdint cimport uint8_t, uint32_t, uint64_t
 from libcpp cimport bool
+from libcpp.map cimport map
 from libcpp.pair cimport pair
+from libcpp.set cimport set
 from libcpp.string cimport string
 from libcpp.vector cimport vector
 from libcpp.unordered_map cimport unordered_map
@@ -53,9 +55,7 @@ cdef extern from "common_dbc.h":
     unordered_map[string, const Msg*] name_to_msg
 
   cdef struct SignalValue:
-    uint32_t address
     uint64_t ts_nanos
-    string name
     double value
     vector[double] all_values
 
@@ -67,6 +67,9 @@ cdef extern from "common_dbc.h":
 cdef extern from "common.h":
   cdef const DBC* dbc_lookup(const string) except +
 
+  cdef cppclass MessageState:
+    map[string, SignalValue] values
+
   cdef struct CanFrame:
     long src
     uint32_t address
@@ -80,7 +83,8 @@ cdef extern from "common.h":
     bool can_valid
     bool bus_timeout
     CANParser(int, string, vector[pair[uint32_t, int]]) except +
-    void update(vector[CanData]&, vector[SignalValue]&) except +
+    MessageState *messageState(uint32_t address)
+    set[uint32_t] update(vector[CanData]&) except +
 
   cdef cppclass CANPacker:
    CANPacker(string)

opendbc/can/common_dbc.h

@@ -11,9 +11,7 @@ struct SignalPackValue {
 };
 
 struct SignalValue {
-  uint32_t address;
   uint64_t ts_nanos;
-  std::string name;
   double value;  // latest value
   std::vector<double> all_values;  // all values from this cycle
 };

opendbc/can/parser.cc

@@ -5,11 +5,6 @@
 #include <stdexcept>
 #include <sstream>
 
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/stat.h>
-#include <sys/mman.h>
-
 #include "opendbc/can/common.h"
 
 int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
@@ -31,7 +26,6 @@ int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
   return ret;
 }
 
-
 bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
   std::vector<double> tmp_vals(parse_sigs.size());
   bool checksum_failed = false;
@@ -69,8 +63,10 @@ bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
   }
 
   for (int i = 0; i < parse_sigs.size(); i++) {
-    vals[i] = tmp_vals[i];
-    all_vals[i].push_back(vals[i]);
+    auto &val = values[parse_sigs[i].name];
+    val.value = tmp_vals[i];
+    val.ts_nanos = nanos;
+    val.all_values.push_back(val.value);
   }
   last_seen_nanos = nanos;
 
@@ -126,8 +122,9 @@ CANParser::CANParser(int abus, const std::string& dbc_name, const std::vector<st
 
     // track all signals for this message
     state.parse_sigs = msg->sigs;
-    state.vals.resize(msg->sigs.size());
-    state.all_vals.resize(msg->sigs.size());
+    for (auto &sig : msg->sigs) {
+      state.values[sig.name] = {};
+    }
   }
 }
 
@@ -147,44 +144,49 @@ CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum
       .ignore_counter = ignore_counter,
     };
 
-    for (const auto& sig : msg.sigs) {
-      state.parse_sigs.push_back(sig);
-      state.vals.push_back(0);
-      state.all_vals.push_back({});
-    }
-
+    state.parse_sigs = msg.sigs;
     message_states[state.address] = state;
+    for (auto &sig : msg.sigs) {
+      message_states[state.address].values[sig.name] = {};
+    }
   }
 }
 
-void CANParser::update(const std::vector<CanData> &can_data, std::vector<SignalValue> &vals) {
-  uint64_t current_nanos = 0;
-  for (const auto &c : can_data) {
-    if (first_nanos == 0) {
-      first_nanos = c.nanos;
-    }
-    if (current_nanos == 0) {
-      current_nanos = c.nanos;
+std::set<uint32_t> CANParser::update(const std::vector<CanData> &can_data) {
+    // Clear all_values
+  for (auto &state : message_states) {
+    for (auto &value : state.second.values) {
+      value.second.all_values.clear();
     }
-    last_nanos = c.nanos;
+  }
+
+  std::set<uint32_t> updated_addresses;
+  if (can_data.empty()) {
+    return updated_addresses;
+  }
 
-    UpdateCans(c);
-    UpdateValid(last_nanos);
+  if (first_nanos == 0) {
+    first_nanos = can_data.front().nanos;
+  }
+
+  for (const auto &c : can_data) {
+    last_nanos = c.nanos;
+    updateCans(c, updated_addresses);
   }
-  query_latest(vals, current_nanos);
+  UpdateValid(last_nanos);
+
+  return updated_addresses;
 }
 
-void CANParser::UpdateCans(const CanData &can) {
+void CANParser::updateCans(const CanData &can, std::set<uint32_t> &updated_addresses) {
   //DEBUG("got %zu messages\n", can.frames.size());
 
-  bool bus_empty = true;
-
   for (const auto &frame : can.frames) {
     if (frame.src != bus) {
       // DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
       continue;
     }
-    bus_empty = false;
+    last_nonempty_nanos = can.nanos;
 
     auto state_it = message_states.find(frame.address);
     if (state_it == message_states.end()) {
@@ -202,14 +204,10 @@ void CANParser::UpdateCans(const CanData &can) {
     //  continue;
     //}
 
-    state_it->second.parse(can.nanos, frame.dat);
-  }
-
-  // update bus timeout
-  if (!bus_empty) {
-    last_nonempty_nanos = can.nanos;
+    if (state_it->second.parse(can.nanos, frame.dat)) {
+      updated_addresses.insert(frame.address);
+    }
   }
-  bus_timeout = (can.nanos - last_nonempty_nanos) > bus_timeout_threshold;
 }
 
 void CANParser::UpdateValid(uint64_t nanos) {
@@ -239,27 +237,5 @@ void CANParser::UpdateValid(uint64_t nanos) {
   }
   can_invalid_cnt = _valid ? 0 : (can_invalid_cnt + 1);
   can_valid = (can_invalid_cnt < CAN_INVALID_CNT) && _counters_valid;
-}
-
-void CANParser::query_latest(std::vector<SignalValue> &vals, uint64_t last_ts) {
-  if (last_ts == 0) {
-    last_ts = last_nanos;
-  }
-  for (auto& kv : message_states) {
-    auto& state = kv.second;
-    if (last_ts != 0 && state.last_seen_nanos < last_ts) {
-      continue;
-    }
-
-    for (int i = 0; i < state.parse_sigs.size(); i++) {
-      const Signal &sig = state.parse_sigs[i];
-      SignalValue &v = vals.emplace_back();
-      v.address = state.address;
-      v.ts_nanos = state.last_seen_nanos;
-      v.name = sig.name;
-      v.value = state.vals[i];
-      v.all_values = state.all_vals[i];
-      state.all_vals[i].clear();
-    }
-  }
+  bus_timeout = (nanos - last_nonempty_nanos) > bus_timeout_threshold;
 }