Modified can parser to run it from PlotJuggler (#352)

* Modified can parser to run it from PlotJuggler

* Assert for parsing DynamicStruct, can ignore checksum/counter messages

* Only write processed dbc on changes, to prevent cmake compiling

* Do not fail on creating new DBC outputs

* Remove print statement

* outer ignore_checksum and ignore_counter

* whitespace
This commit is contained in:
Joost Wooning 2021-03-09 07:33:37 -08:00 committed by GitHub
parent ae058828cf
commit f133708455
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 155 additions and 83 deletions

View File

@ -5,8 +5,12 @@
#include <unordered_map>
#include "common_dbc.h"
#include <capnp/dynamic.h>
#include <capnp/serialize.h>
#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<MessageParseOptions> &options,
const std::vector<SignalParseOptions> &sigoptions);
void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::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<cereal::CanData>::Reader& cans);
#endif
void UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cans);
void UpdateValid(uint64_t sec);
std::vector<SignalValue> query_latest();
};
@ -69,5 +80,6 @@ private:
public:
CANPacker(const std::string& dbc_name);
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter);
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &values, int counter);
Msg* lookup_message(uint32_t address);
};

View File

@ -3,6 +3,7 @@
#include <cstddef>
#include <cstdint>
#include <string>
#include <vector>
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
@ -74,6 +75,7 @@ struct DBC {
size_t num_vals;
};
std::vector<const DBC*>& get_dbcs();
const DBC* dbc_lookup(const std::string& dbc_name);
void dbc_register(const DBC* dbc);

View File

@ -2,15 +2,11 @@
#include "common_dbc.h"
namespace {
std::vector<const DBC*>& get_dbcs() {
static std::vector<const DBC*> vec;
return vec;
}
}
const DBC* dbc_lookup(const std::string& dbc_name) {
for (const auto& dbci : get_dbcs()) {
if (dbc_name == dbci->name) {

View File

@ -112,3 +112,7 @@ uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &s
return ret;
}
Msg* CANPacker::lookup_message(uint32_t address) {
return &message_lookup[address];
}

View File

@ -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; i<dbc->num_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; i<msg->num_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; i<msg->num_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<cereal::CanData>::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<cereal::CanData>::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<uint8_t>() != bus) {
DEBUG("skip %d: wrong bus\n", cmsg.get("address").as<uint32_t>());
return;
}
auto state_it = message_states.find(cmsg.get("address").as<uint32_t>());
if (state_it == message_states.end()) {
DEBUG("skip %d: not specified\n", cmsg.get("address").as<uint32_t>());
return;
}
auto dat = cmsg.get("dat").as<capnp::Data>();
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<uint16_t>(), 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<SignalValue> CANParser::query_latest() {
std::vector<SignalValue> ret;

View File

@ -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: