diff --git a/can/common.h b/can/common.h index e9cf591..9583340 100644 --- a/can/common.h +++ b/can/common.h @@ -5,8 +5,12 @@ #include #include "common_dbc.h" +#include #include + +#ifndef DYNAMIC_CAPNP #include "cereal/gen/cpp/log.capnp.h" +#endif #define MAX_BAD_COUNTER 5 @@ -36,6 +40,9 @@ public: uint8_t counter; uint8_t counter_fail; + bool ignore_checksum = false; + bool ignore_counter = false; + bool parse(uint64_t sec, uint16_t ts_, uint8_t * dat); bool update_counter_generic(int64_t v, int cnt_size); }; @@ -55,9 +62,13 @@ public: CANParser(int abus, const std::string& dbc_name, const std::vector &options, const std::vector &sigoptions); - void UpdateCans(uint64_t sec, const capnp::List::Reader& cans); - void UpdateValid(uint64_t sec); + 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 UpdateCans(uint64_t sec, const capnp::List::Reader& cans); + #endif + void UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cans); + void UpdateValid(uint64_t sec); std::vector query_latest(); }; @@ -69,5 +80,6 @@ private: public: CANPacker(const std::string& dbc_name); - uint64_t pack(uint32_t address, const std::vector &signals, int counter); + uint64_t pack(uint32_t address, const std::vector &values, int counter); + Msg* lookup_message(uint32_t address); }; diff --git a/can/common_dbc.h b/can/common_dbc.h index e5c12ba..dc49f31 100644 --- a/can/common_dbc.h +++ b/can/common_dbc.h @@ -3,6 +3,7 @@ #include #include #include +#include #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) @@ -74,6 +75,7 @@ struct DBC { size_t num_vals; }; +std::vector& get_dbcs(); const DBC* dbc_lookup(const std::string& dbc_name); void dbc_register(const DBC* dbc); diff --git a/can/dbc.cc b/can/dbc.cc index 6587de7..5747c7c 100644 --- a/can/dbc.cc +++ b/can/dbc.cc @@ -2,15 +2,11 @@ #include "common_dbc.h" -namespace { - std::vector& get_dbcs() { static std::vector vec; return vec; } -} - const DBC* dbc_lookup(const std::string& dbc_name) { for (const auto& dbci : get_dbcs()) { if (dbc_name == dbci->name) { diff --git a/can/packer.cc b/can/packer.cc index 0082fe9..146b4de 100644 --- a/can/packer.cc +++ b/can/packer.cc @@ -112,3 +112,7 @@ uint64_t CANPacker::pack(uint32_t address, const std::vector &s return ret; } + +Msg* CANPacker::lookup_message(uint32_t address) { + return &message_lookup[address]; +} diff --git a/can/parser.cc b/can/parser.cc index a41850b..9e23311 100644 --- a/can/parser.cc +++ b/can/parser.cc @@ -13,7 +13,6 @@ // #define DEBUG printf #define INFO printf - bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) { uint64_t dat_le = read_u64_le(dat); uint64_t dat_be = read_u64_be(dat); @@ -34,47 +33,52 @@ bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) { DEBUG("parse 0x%X %s -> %lld\n", address, sig.name, tmp); - if (sig.type == SignalType::HONDA_CHECKSUM) { - if (honda_checksum(address, dat_be, size) != tmp) { - INFO("0x%X CHECKSUM FAIL\n", address); - return false; + if (!ignore_checksum) { + if (sig.type == SignalType::HONDA_CHECKSUM) { + if (honda_checksum(address, dat_be, size) != tmp) { + INFO("0x%X CHECKSUM FAIL\n", address); + return false; + } + } else if (sig.type == SignalType::TOYOTA_CHECKSUM) { + if (toyota_checksum(address, dat_be, size) != tmp) { + INFO("0x%X CHECKSUM FAIL\n", address); + return false; + } + } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) { + if (volkswagen_crc(address, dat_le, size) != tmp) { + INFO("0x%X CRC FAIL\n", address); + return false; + } + } else if (sig.type == SignalType::SUBARU_CHECKSUM) { + if (subaru_checksum(address, dat_be, size) != tmp) { + INFO("0x%X CHECKSUM FAIL\n", address); + return false; + } + } else if (sig.type == SignalType::CHRYSLER_CHECKSUM) { + if (chrysler_checksum(address, dat_le, size) != tmp) { + INFO("0x%X CHECKSUM FAIL\n", address); + return false; + } + } else if (sig.type == SignalType::PEDAL_CHECKSUM) { + if (pedal_checksum(dat_be, size) != tmp) { + INFO("0x%X PEDAL CHECKSUM FAIL\n", address); + return false; + } } - } else if (sig.type == SignalType::HONDA_COUNTER) { - if (!update_counter_generic(tmp, sig.b2)) { - return false; - } - } else if (sig.type == SignalType::TOYOTA_CHECKSUM) { - if (toyota_checksum(address, dat_be, size) != tmp) { - INFO("0x%X CHECKSUM FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) { - if (volkswagen_crc(address, dat_le, size) != tmp) { - INFO("0x%X CRC FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) { + } + if (!ignore_counter) { + if (sig.type == SignalType::HONDA_COUNTER) { if (!update_counter_generic(tmp, sig.b2)) { - return false; - } - } else if (sig.type == SignalType::SUBARU_CHECKSUM) { - if (subaru_checksum(address, dat_be, size) != tmp) { - INFO("0x%X CHECKSUM FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::CHRYSLER_CHECKSUM) { - if (chrysler_checksum(address, dat_le, size) != tmp) { - INFO("0x%X CHECKSUM FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::PEDAL_CHECKSUM) { - if (pedal_checksum(dat_be, size) != tmp) { - INFO("0x%X PEDAL CHECKSUM FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::PEDAL_COUNTER) { - if (!update_counter_generic(tmp, sig.b2)) { - return false; + return false; + } + } else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) { + if (!update_counter_generic(tmp, sig.b2)) { + return false; + } + } else if (sig.type == SignalType::PEDAL_COUNTER) { + if (!update_counter_generic(tmp, sig.b2)) { + return false; + } } } @@ -125,7 +129,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name, } const Msg* msg = NULL; - for (int i=0; inum_msgs; i++) { + for (int i = 0; i < dbc->num_msgs; i++) { if (dbc->msgs[i].address == op.address) { msg = &dbc->msgs[i]; break; @@ -139,7 +143,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name, state.size = msg->size; // track checksums and counters for this message - for (int i=0; inum_sigs; i++) { + for (int i = 0; i < msg->num_sigs; i++) { const Signal *sig = &msg->sigs[i]; if (sig->type != SignalType::DEFAULT) { state.parse_sigs.push_back(*sig); @@ -151,7 +155,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name, for (const auto& sigop : sigoptions) { if (sigop.address != op.address) continue; - for (int i=0; inum_sigs; i++) { + for (int i = 0; i < msg->num_sigs; i++) { const Signal *sig = &msg->sigs[i]; if (strcmp(sig->name, sigop.name) == 0 && sig->type == SignalType::DEFAULT) { @@ -164,45 +168,34 @@ CANParser::CANParser(int abus, const std::string& dbc_name, } } -void CANParser::UpdateCans(uint64_t sec, const capnp::List::Reader& cans) { - int msg_count = cans.size(); +CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter) + : bus(abus) { + // Add all messages and signals - DEBUG("got %d messages\n", msg_count); + dbc = dbc_lookup(dbc_name); + assert(dbc); + init_crc_lookup_tables(); - // parse the messages - for (int i = 0; i < msg_count; i++) { - auto cmsg = cans[i]; - if (cmsg.getSrc() != bus) { - // DEBUG("skip %d: wrong bus\n", cmsg.getAddress()); - continue; - } - auto state_it = message_states.find(cmsg.getAddress()); - if (state_it == message_states.end()) { - // DEBUG("skip %d: not specified\n", cmsg.getAddress()); - continue; - } + for (int i = 0; i < dbc->num_msgs; i++) { + const Msg* msg = &dbc->msgs[i]; + MessageState state = { + .address = msg->address, + .size = msg->size, + .ignore_checksum = ignore_checksum, + .ignore_counter = ignore_counter, + }; - if (cmsg.getDat().size() > 8) continue; //shouldn't ever happen - uint8_t dat[8] = {0}; - memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); - - state_it->second.parse(sec, cmsg.getBusTime(), dat); + for (int j = 0; j < msg->num_sigs; j++) { + const Signal *sig = &msg->sigs[j]; + state.parse_sigs.push_back(*sig); + state.vals.push_back(0); } -} -void CANParser::UpdateValid(uint64_t sec) { - can_valid = true; - for (const auto& kv : message_states) { - const auto& state = kv.second; - if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { - if (state.seen > 0) { - DEBUG("0x%X TIMEOUT\n", state.address); - } - can_valid = false; - } + message_states[state.address] = state; } } +#ifndef DYNAMIC_CAPNP void CANParser::update_string(const std::string &data, bool sendcan) { // format for board, make copy due to alignment issues. const size_t buf_size = (data.length() / sizeof(capnp::word)) + 1; @@ -223,6 +216,67 @@ void CANParser::update_string(const std::string &data, bool sendcan) { UpdateValid(last_sec); } +void CANParser::UpdateCans(uint64_t sec, const capnp::List::Reader& cans) { + int msg_count = cans.size(); + + DEBUG("got %d messages\n", msg_count); + + for (int i = 0; i < msg_count; i++) { + auto cmsg = cans[i]; + // parse the messages + if (cmsg.getSrc() != bus) { + // DEBUG("skip %d: wrong bus\n", cmsg.getAddress()); + continue; + } + auto state_it = message_states.find(cmsg.getAddress()); + if (state_it == message_states.end()) { + // DEBUG("skip %d: not specified\n", cmsg.getAddress()); + continue; + } + + if (cmsg.getDat().size() > 8) continue; //shouldn't ever happen + uint8_t dat[8] = {0}; + memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); + + state_it->second.parse(sec, cmsg.getBusTime(), dat); + } +} +#endif + +void CANParser::UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cmsg) { + // assume message struct is `cereal::CanData` and parse + assert(cmsg.has("address") && cmsg.has("src") && cmsg.has("dat") && cmsg.has("busTime")); + + if (cmsg.get("src").as() != bus) { + DEBUG("skip %d: wrong bus\n", cmsg.get("address").as()); + return; + } + + auto state_it = message_states.find(cmsg.get("address").as()); + if (state_it == message_states.end()) { + DEBUG("skip %d: not specified\n", cmsg.get("address").as()); + return; + } + + auto dat = cmsg.get("dat").as(); + if (dat.size() > 8) return; //shouldn't ever happen + uint8_t data[8] = {0}; + memcpy(data, dat.begin(), dat.size()); + state_it->second.parse(sec, cmsg.get("busTime").as(), data); +} + +void CANParser::UpdateValid(uint64_t sec) { + can_valid = true; + for (const auto& kv : message_states) { + const auto& state = kv.second; + if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { + if (state.seen > 0) { + DEBUG("0x%X TIMEOUT\n", state.address); + } + can_valid = false; + } + } +} std::vector CANParser::query_latest() { std::vector ret; diff --git a/can/process_dbc.py b/can/process_dbc.py index 9025331..811e59b 100755 --- a/can/process_dbc.py +++ b/can/process_dbc.py @@ -106,8 +106,12 @@ def process(in_fn, out_fn): parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len) - with open(out_fn, "w") as out_f: - out_f.write(parser_code) + with open(out_fn, "a+") as out_f: + out_f.seek(0) + if out_f.read() != parser_code: + out_f.seek(0) + out_f.truncate() + out_f.write(parser_code) def main(): if len(sys.argv) != 3: