diff --git a/can/common.h b/can/common.h index 081c32e3ed..df669d4d0f 100644 --- a/can/common.h +++ b/can/common.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -43,8 +44,7 @@ class MessageState { unsigned int size; std::vector parse_sigs; - std::vector vals; - std::vector> all_vals; + std::map values; uint64_t last_seen_nanos; uint64_t check_threshold; @@ -66,6 +66,7 @@ class CANParser { const DBC *dbc = NULL; std::unordered_map message_states; + kj::ArrayPtr getAlignedData(const std::string &data); public: bool can_valid = false; @@ -80,13 +81,12 @@ class CANParser { const std::vector> &messages); CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter); #ifndef DYNAMIC_CAPNP - void update_string(const std::string &data, bool sendcan); - void update_strings(const std::vector &data, std::vector &vals, bool sendcan); - void UpdateCans(uint64_t nanos, const capnp::List::Reader& cans); + MessageState *messageState(uint32_t address) { return &message_states.at(address); } + std::set update_strings(const std::vector &data, bool sendcan); + void UpdateCans(uint64_t nanos, const capnp::List::Reader& cans, std::set updated_addresses); #endif void UpdateCans(uint64_t nanos, const capnp::DynamicStruct::Reader& cans); void UpdateValid(uint64_t nanos); - void query_latest(std::vector &vals, uint64_t last_ts = 0); }; class CANPacker { diff --git a/can/common.pxd b/can/common.pxd index 4dab92cd5f..f35d063752 100644 --- a/can/common.pxd +++ b/can/common.pxd @@ -3,7 +3,9 @@ from libc.stdint cimport uint8_t, uint16_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,11 +67,15 @@ 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 cppclass CANParser: bool can_valid bool bus_timeout CANParser(int, string, vector[pair[uint32_t, int]]) except + - void update_strings(vector[string]&, vector[SignalValue]&, bool) except + + MessageState *messageState(uint32_t address) + set[uint32_t] update_strings(vector[string]&, bool) except + cdef cppclass CANPacker: CANPacker(string) diff --git a/can/common_dbc.h b/can/common_dbc.h index 19507ecd4e..94e974b671 100644 --- a/can/common_dbc.h +++ b/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 all_values; // all values from this cycle }; diff --git a/can/parser.cc b/can/parser.cc index 2d1688caaa..fba2aa13ff 100644 --- a/can/parser.cc +++ b/can/parser.cc @@ -5,11 +5,6 @@ #include #include -#include -#include -#include -#include - #include "opendbc/can/common.h" int64_t get_raw_value(const std::vector &msg, const Signal &sig) { @@ -31,13 +26,12 @@ int64_t get_raw_value(const std::vector &msg, const Signal &sig) { return ret; } - bool MessageState::parse(uint64_t nanos, const std::vector &dat) { std::vector tmp_vals(parse_sigs.size()); bool checksum_failed = false; bool counter_failed = false; - for (int i = 0; i < parse_sigs.size(); i++) { + for (int i = 0; i < parse_sigs.size(); ++i) { const auto &sig = parse_sigs[i]; int64_t tmp = get_raw_value(dat, sig); @@ -68,9 +62,11 @@ bool MessageState::parse(uint64_t nanos, const std::vector &dat) { return false; } - for (int i = 0; i < parse_sigs.size(); i++) { - vals[i] = tmp_vals[i]; - all_vals[i].push_back(vals[i]); + for (int i = 0; i < parse_sigs.size(); ++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; @@ -127,8 +123,9 @@ CANParser::CANParser(int abus, const std::string& dbc_name, const std::vectorsigs; - state.vals.resize(msg->sigs.size()); - state.all_vals.resize(msg->sigs.size()); + for (auto &sig : msg->sigs) { + state.values[sig.name] = {}; + } } } @@ -149,54 +146,51 @@ 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; + // Initialize value entries for each signal in the message + for (auto &sig : msg.sigs) { + message_states[state.address].values[sig.name] = {}; + } } } #ifndef DYNAMIC_CAPNP -void CANParser::update_string(const std::string &data, bool sendcan) { - // format for board, make copy due to alignment issues. + kj::ArrayPtr CANParser::getAlignedData(const std::string &data) { const size_t buf_size = (data.length() / sizeof(capnp::word)) + 1; if (aligned_buf.size() < buf_size) { aligned_buf = kj::heapArray(buf_size); } memcpy(aligned_buf.begin(), data.data(), data.length()); + return aligned_buf.slice(0, buf_size); +} - // extract the messages - capnp::FlatArrayMessageReader cmsg(aligned_buf.slice(0, buf_size)); - cereal::Event::Reader event = cmsg.getRoot(); - - if (first_nanos == 0) { - first_nanos = event.getLogMonoTime(); +std::set CANParser::update_strings(const std::vector &data, bool sendcan) { + // Clear all_values + for (auto &state : message_states) { + for (auto &value : state.second.values) { + value.second.all_values.clear(); + } } - last_nanos = event.getLogMonoTime(); - - auto cans = sendcan ? event.getSendcan() : event.getCan(); - UpdateCans(last_nanos, cans); - UpdateValid(last_nanos); -} - -void CANParser::update_strings(const std::vector &data, std::vector &vals, bool sendcan) { - uint64_t current_nanos = 0; + std::set updated_addresses; for (const auto &d : data) { - update_string(d, sendcan); - if (current_nanos == 0) { - current_nanos = last_nanos; + capnp::FlatArrayMessageReader cmsg(getAlignedData(d)); + cereal::Event::Reader event = cmsg.getRoot(); + + if (first_nanos == 0) { + first_nanos = event.getLogMonoTime(); } + last_nanos = event.getLogMonoTime(); + + auto cans = sendcan ? event.getSendcan() : event.getCan(); + UpdateCans(last_nanos, cans, updated_addresses); + UpdateValid(last_nanos); } - query_latest(vals, current_nanos); + return updated_addresses; } -void CANParser::UpdateCans(uint64_t nanos, const capnp::List::Reader& cans) { - //DEBUG("got %d messages\n", cans.size()); - +void CANParser::UpdateCans(uint64_t nanos, const capnp::List::Reader& cans, std::set updated_addresses) { bool bus_empty = true; // parse the messages @@ -214,7 +208,6 @@ void CANParser::UpdateCans(uint64_t nanos, const capnp::List::R } auto dat = cmsg.getDat(); - if (dat.size() > 64) { DEBUG("got message longer than 64 bytes: 0x%X %zu\n", cmsg.getAddress(), dat.size()); continue; @@ -231,7 +224,9 @@ void CANParser::UpdateCans(uint64_t nanos, const capnp::List::R size_t data_size = std::max(dat.size(), state_it->second.size); std::vector data(data_size, 0); memcpy(data.data(), dat.begin(), dat.size()); - state_it->second.parse(nanos, data); + if (state_it->second.parse(nanos, data)) { + updated_addresses.insert(state_it->first); + } } // update bus timeout @@ -292,26 +287,3 @@ 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 &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(); - } - } -} diff --git a/can/parser_pyx.pyx b/can/parser_pyx.pyx index a5b802dfce..e7be7bcc1c 100644 --- a/can/parser_pyx.pyx +++ b/can/parser_pyx.pyx @@ -1,25 +1,63 @@ # distutils: language = c++ # cython: c_string_encoding=ascii, language_level=3 -from cython.operator cimport dereference as deref, preincrement as preinc from libcpp.pair cimport pair from libcpp.string cimport string from libcpp.vector cimport vector from libc.stdint cimport uint32_t from .common cimport CANParser as cpp_CANParser -from .common cimport dbc_lookup, SignalValue, DBC +from .common cimport MessageState as cpp_MessageState +from .common cimport dbc_lookup, DBC import numbers from collections import defaultdict +from collections.abc import Mapping + +cdef class MessageState: + cdef cpp_MessageState *state + cdef list signal_names + + @property + def names(self): + return self.signal_names + + def value(self, name): + return self.state.values.at(name).value + + def all_values(self, name): + return self.state.values.at(name).all_values + + def ts_nanos(self, name): + return self.state.values.at(name).ts_nanos + + @staticmethod + cdef create(cpp_MessageState *s): + state = MessageState() + state.state = s + state.signal_names = [it.first.decode("utf-8") for it in s.values] + return state + + +class ValueDict(Mapping): + def __init__(self, MessageState state, fetch_func): + self.state = state + self.fetch_func = fetch_func + + def __getitem__(self, key): + return self.fetch_func(self.state, key) + + def __iter__(self): + return iter(self.state.names) + + def __len__(self): + return len(self.state.names) cdef class CANParser: cdef: cpp_CANParser *can const DBC *dbc - vector[SignalValue] can_values - vector[uint32_t] addresses cdef readonly: dict vl @@ -48,56 +86,22 @@ cdef class CANParser: address = m.address message_v.push_back((address, c[1])) - self.addresses.push_back(address) - - name = m.name.decode("utf8") - self.vl[address] = {} - self.vl[name] = self.vl[address] - self.vl_all[address] = defaultdict(list) - self.vl_all[name] = self.vl_all[address] - self.ts_nanos[address] = {} - self.ts_nanos[name] = self.ts_nanos[address] self.can = new cpp_CANParser(bus, dbc_name, message_v) - self.update_strings([]) + + for address, _ in message_v: + name = address_to_msg_name[address] + state = MessageState.create(self.can.messageState(address)) + self.vl[name] = self.vl[address] = ValueDict(state, MessageState.value) + self.vl_all[name] = self.vl_all[address] = ValueDict(state, MessageState.all_values) + self.ts_nanos[name] = self.ts_nanos[address] = ValueDict(state, MessageState.ts_nanos) def __dealloc__(self): if self.can: del self.can def update_strings(self, strings, sendcan=False): - for address in self.addresses: - self.vl_all[address].clear() - - cdef vector[SignalValue] new_vals - cur_address = -1 - vl = {} - vl_all = {} - ts_nanos = {} - updated_addrs = set() - - self.can.update_strings(strings, new_vals, sendcan) - cdef vector[SignalValue].iterator it = new_vals.begin() - cdef SignalValue* cv - while it != new_vals.end(): - cv = &deref(it) - - # Check if the address has changed - if cv.address != cur_address: - cur_address = cv.address - vl = self.vl[cur_address] - vl_all = self.vl_all[cur_address] - ts_nanos = self.ts_nanos[cur_address] - updated_addrs.add(cur_address) - - # Cast char * directly to unicode - cv_name = cv.name - vl[cv_name] = cv.value - vl_all[cv_name] = cv.all_values - ts_nanos[cv_name] = cv.ts_nanos - preinc(it) - - return updated_addrs + return self.can.update_strings(strings, sendcan) @property def can_valid(self):