From e155e017489ce3c5d6f46232c667b5c58221f127 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Fri, 22 Nov 2019 21:46:33 -0800 Subject: [PATCH] Can migration (#199) * moved CAN to opendbc * also dbc was needed * dbc test also added --- can/Makefile | 97 +++++++++++ can/__init__.py | 0 can/can_define.py | 39 +++++ can/cereal.mk | 38 +++++ can/common.cc | 165 +++++++++++++++++++ can/common.h | 59 +++++++ can/common.pxd | 73 +++++++++ can/common_dbc.h | 82 ++++++++++ can/dbc.cc | 31 ++++ can/dbc.py | 276 ++++++++++++++++++++++++++++++++ can/dbc_out/.gitignore | 2 + can/dbc_out/.gitkeep | 0 can/dbc_template.cc | 81 ++++++++++ can/dbc_test.py | 33 ++++ can/libdbc_py.py | 95 +++++++++++ can/packer.cc | 139 ++++++++++++++++ can/packer.py | 9 ++ can/packer_impl.pyx | 124 ++++++++++++++ can/packer_setup.py | 9 ++ can/parser.cc | 239 +++++++++++++++++++++++++++ can/parser.py | 9 ++ can/parser_pyx.pyx | 138 ++++++++++++++++ can/parser_pyx_setup.py | 35 ++++ can/process_dbc.py | 105 ++++++++++++ can/tests/.gitignore | 1 + can/tests/__init__.py | 0 can/tests/test_packer_parser.py | 85 ++++++++++ 27 files changed, 1964 insertions(+) create mode 100644 can/Makefile create mode 100644 can/__init__.py create mode 100644 can/can_define.py create mode 100644 can/cereal.mk create mode 100644 can/common.cc create mode 100644 can/common.h create mode 100644 can/common.pxd create mode 100644 can/common_dbc.h create mode 100644 can/dbc.cc create mode 100755 can/dbc.py create mode 100644 can/dbc_out/.gitignore create mode 100644 can/dbc_out/.gitkeep create mode 100644 can/dbc_template.cc create mode 100644 can/dbc_test.py create mode 100644 can/libdbc_py.py create mode 100644 can/packer.cc create mode 100644 can/packer.py create mode 100644 can/packer_impl.pyx create mode 100644 can/packer_setup.py create mode 100644 can/parser.cc create mode 100644 can/parser.py create mode 100644 can/parser_pyx.pyx create mode 100644 can/parser_pyx_setup.py create mode 100755 can/process_dbc.py create mode 100644 can/tests/.gitignore create mode 100644 can/tests/__init__.py create mode 100644 can/tests/test_packer_parser.py diff --git a/can/Makefile b/can/Makefile new file mode 100644 index 0000000..333b67c --- /dev/null +++ b/can/Makefile @@ -0,0 +1,97 @@ +CC = clang +CXX = clang++ + +BASEDIR = ../.. +PHONELIBS := ../../phonelibs + +UNAME_S := $(shell uname -s) +UNAME_M := $(shell uname -m) + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args \ + -Wno-deprecated-declarations + +CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) +LDFLAGS = + +ifeq ($(ARCH),aarch64) +CFLAGS += -mcpu=cortex-a57 +CXXFLAGS += -mcpu=cortex-a57 +endif + +OBJDIR = obj + +OPENDBC_PATH := $(shell python3 -c 'import opendbc; print(opendbc.DBC_PATH)') + +DBC_SOURCES := $(sort $(wildcard $(OPENDBC_PATH)/*.dbc)) +DBC_OBJS := $(patsubst $(OPENDBC_PATH)/%.dbc,$(OBJDIR)/%.o,$(DBC_SOURCES)) +DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES)) +.SECONDARY: $(DBC_CCS) + +LIBDBC_OBJS := $(OBJDIR)/dbc.o $(OBJDIR)/parser.o $(OBJDIR)/packer.o $(OBJDIR)/common.o + +CWD := $(shell pwd) + +.PHONY: all +all: $(OBJDIR) libdbc.so parser_pyx.so + +include cereal.mk + +# make sure cereal is built +libdbc.so:: ../../cereal/gen/cpp/log.capnp.h + +../../cereal/gen/cpp/log.capnp.h: + cd ../../cereal && make + +libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS) + @echo "[ LINK ] $@" + $(CXX) -fPIC -shared -o '$@' $^ \ + -I. -I../.. \ + $(CXXFLAGS) \ + $(LDFLAGS) \ + $(CEREAL_CXXFLAGS) \ + $(CEREAL_LIBS) + +packer_impl.so: packer_impl.pyx packer_setup.py + python3 packer_setup.py build_ext --inplace + rm -rf build + rm -f packer_impl.cpp + +parser_pyx.so: libdbc.so parser_pyx_setup.py parser_pyx.pyx common.pxd + python3 parser_pyx_setup.py build_ext --inplace + rm -rf build + rm -f parser_pyx.cpp + +$(OBJDIR)/%.o: %.cc + @echo "[ CXX ] $@" + $(CXX) -fPIC -c -o '$@' $^ \ + -I. -I../.. \ + $(CXXFLAGS) \ + $(CEREAL_CXXFLAGS) \ + +$(OBJDIR)/%.o: dbc_out/%.cc + @echo "[ CXX ] $@" + $(CXX) -fPIC -c -o '$@' $^ \ + -I. -I../.. \ + $(CXXFLAGS) \ + $(CEREAL_CXXFLAGS) \ + +dbc_out/%.cc: process_dbc.py dbc_template.cc $(OPENDBC_PATH)/%.dbc + @echo "[ DBC GEN ] $@" + ./process_dbc.py $(OPENDBC_PATH) '$@' + +$(OBJDIR): + mkdir -p $@ + +.PHONY: clean $(OBJDIR) +clean: + rm -rf libdbc.so* + rm -f dbc_out/*.cc + rm -f dbcs.txt + rm -f dbcs.csv + rm -f *.so + rm -rf $(OBJDIR)/* diff --git a/can/__init__.py b/can/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/can/can_define.py b/can/can_define.py new file mode 100644 index 0000000..4b6f679 --- /dev/null +++ b/can/can_define.py @@ -0,0 +1,39 @@ +from collections import defaultdict +from opendbc.can.libdbc_py import libdbc, ffi + +class CANDefine(): + def __init__(self, dbc_name): + self.dv = defaultdict(dict) + self.dbc_name = dbc_name + self.dbc = libdbc.dbc_lookup(dbc_name.encode('utf8')) + + num_vals = self.dbc[0].num_vals + + self.address_to_msg_name = {} + num_msgs = self.dbc[0].num_msgs + for i in range(num_msgs): + msg = self.dbc[0].msgs[i] + name = ffi.string(msg.name).decode('utf8') + address = msg.address + self.address_to_msg_name[address] = name + + for i in range(num_vals): + val = self.dbc[0].vals[i] + + sgname = ffi.string(val.name).decode('utf8') + address = val.address + def_val = ffi.string(val.def_val).decode('utf8') + + #separate definition/value pairs + def_val = def_val.split() + values = [int(v) for v in def_val[::2]] + defs = def_val[1::2] + + if address not in self.dv: + self.dv[address] = {} + msgname = self.address_to_msg_name[address] + self.dv[msgname] = {} + + # two ways to lookup: address or msg name + self.dv[address][sgname] = dict(zip(values, defs)) + self.dv[msgname][sgname] = self.dv[address][sgname] diff --git a/can/cereal.mk b/can/cereal.mk new file mode 100644 index 0000000..ed05efc --- /dev/null +++ b/can/cereal.mk @@ -0,0 +1,38 @@ +UNAME_M ?= $(shell uname -m) +UNAME_S ?= $(shell uname -s) + +ifeq ($(UNAME_S),Darwin) +CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include +CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include +CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \ + $(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \ + $(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a + +else ifeq ($(UNAME_M),x86_64) +CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include +CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include +ifeq ($(CEREAL_LIBS),) + CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/x64/lib/ \ + -L$(PHONELIBS)/capnp-c/x64/lib/ \ + -l:libcapnp.a -l:libkj.a -l:libcapnp_c.a +endif + +else + +#CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include +ifeq ($(CEREAL_LIBS),) + CEREAL_LIBS = -l:libcapn.a -l:libcapnp.a -l:libkj.a +endif +endif + +CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/car.capnp.o + +log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++ + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \ + -c -o '$@' '$<' + +car.capnp.o: ../../cereal/gen/cpp/car.capnp.c++ + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \ + -c -o '$@' '$<' diff --git a/can/common.cc b/can/common.cc new file mode 100644 index 0000000..5a131eb --- /dev/null +++ b/can/common.cc @@ -0,0 +1,165 @@ +#include "common.h" + +unsigned int honda_checksum(unsigned int address, uint64_t d, int l) { + d >>= ((8-l)*8); // remove padding + d >>= 4; // remove checksum + + int s = 0; + while (address) { s += (address & 0xF); address >>= 4; } + while (d) { s += (d & 0xF); d >>= 4; } + s = 8-s; + s &= 0xF; + + return s; +} + +unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) { + d >>= ((8-l)*8); // remove padding + d >>= 8; // remove checksum + + unsigned int s = l; + while (address) { s += address & 0xff; address >>= 8; } + while (d) { s += d & 0xff; d >>= 8; } + + return s & 0xFF; +} + +// Static lookup table for fast computation of CRC8 poly 0x2F, aka 8H2F/AUTOSAR +uint8_t crc8_lut_8h2f[256]; + +void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) { + uint8_t crc; + int i, j; + + for (i = 0; i < 256; i++) { + crc = i; + for (j = 0; j < 8; j++) { + if ((crc & 0x80) != 0) + crc = (uint8_t)((crc << 1) ^ poly); + else + crc <<= 1; + } + crc_lut[i] = crc; + } +} + +void init_crc_lookup_tables() { + // At init time, set up static lookup tables for fast CRC computation. + + gen_crc_lookup_table(0x2F, crc8_lut_8h2f); // CRC-8 8H2F/AUTOSAR for Volkswagen +} + +unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) { + // Volkswagen uses standard CRC8 8H2F/AUTOSAR, but they compute it with + // a magic variable padding byte tacked onto the end of the payload. + // https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf + + uint8_t *dat = (uint8_t *)&d; + uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR + + // CRC the payload first, skipping over the first byte where the CRC lives. + for (int i = 1; i < l; i++) { + crc ^= dat[i]; + crc = crc8_lut_8h2f[crc]; + } + + // Look up and apply the magic final CRC padding byte, which permutes by CAN + // address, and additionally (for SOME addresses) by the message counter. + uint8_t counter = dat[1] & 0x0F; + switch(address) { + case 0x86: // LWI_01 Steering Angle + crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter]; + break; + case 0x9F: // EPS_01 Electric Power Steering + crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter]; + break; + case 0xAD: // Getriebe_11 Automatic Gearbox + crc ^= (uint8_t[]){0x3F,0x69,0x39,0xDC,0x94,0xF9,0x14,0x64,0xD8,0x6A,0x34,0xCE,0xA2,0x55,0xB5,0x2C}[counter]; + break; + case 0xFD: // ESP_21 Electronic Stability Program + crc ^= (uint8_t[]){0xB4,0xEF,0xF8,0x49,0x1E,0xE5,0xC2,0xC0,0x97,0x19,0x3C,0xC9,0xF1,0x98,0xD6,0x61}[counter]; + break; + case 0x106: // ESP_05 Electronic Stability Program + crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter]; + break; + case 0x117: // ACC_10 Automatic Cruise Control + crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter]; + break; + case 0x122: // ACC_06 Automatic Cruise Control + crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter]; + break; + case 0x126: // HCA_01 Heading Control Assist + crc ^= (uint8_t[]){0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA}[counter]; + break; + case 0x12B: // GRA_ACC_01 Steering wheel controls for ACC + crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter]; + break; + case 0x187: // EV_Gearshift "Gear" selection data for EVs with no gearbox + crc ^= (uint8_t[]){0x7F,0xED,0x17,0xC2,0x7C,0xEB,0x44,0x21,0x01,0xFA,0xDB,0x15,0x4A,0x6B,0x23,0x05}[counter]; + break; + case 0x30C: // ACC_02 Automatic Cruise Control + crc ^= (uint8_t[]){0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}[counter]; + break; + case 0x3C0: // Klemmen_Status_01 ignition and starting status + crc ^= (uint8_t[]){0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3}[counter]; + break; + case 0x65D: // ESP_20 Electronic Stability Program + crc ^= (uint8_t[]){0xAC,0xB3,0xAB,0xEB,0x7A,0xE1,0x3B,0xF7,0x73,0xBA,0x7C,0x9E,0x06,0x5F,0x02,0xD9}[counter]; + break; + default: // As-yet undefined CAN message, CRC check expected to fail + printf("Attempt to CRC check undefined Volkswagen message 0x%02X\n", address); + crc ^= (uint8_t[]){0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}[counter]; + break; + } + crc = crc8_lut_8h2f[crc]; + + return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR +} + + +unsigned int pedal_checksum(uint64_t d, int l) { + uint8_t crc = 0xFF; + uint8_t poly = 0xD5; // standard crc8 + + d >>= ((8-l)*8); // remove padding + d >>= 8; // remove checksum + + uint8_t *dat = (uint8_t *)&d; + + int i, j; + for (i = 0; i < l - 1; i++) { + crc ^= dat[i]; + for (j = 0; j < 8; j++) { + if ((crc & 0x80) != 0) { + crc = (uint8_t)((crc << 1) ^ poly); + } + else { + crc <<= 1; + } + } + } + return crc; +} + + +uint64_t read_u64_be(const uint8_t* v) { + return (((uint64_t)v[0] << 56) + | ((uint64_t)v[1] << 48) + | ((uint64_t)v[2] << 40) + | ((uint64_t)v[3] << 32) + | ((uint64_t)v[4] << 24) + | ((uint64_t)v[5] << 16) + | ((uint64_t)v[6] << 8) + | (uint64_t)v[7]); +} + +uint64_t read_u64_le(const uint8_t* v) { + return ((uint64_t)v[0] + | ((uint64_t)v[1] << 8) + | ((uint64_t)v[2] << 16) + | ((uint64_t)v[3] << 24) + | ((uint64_t)v[4] << 32) + | ((uint64_t)v[5] << 40) + | ((uint64_t)v[6] << 48) + | ((uint64_t)v[7] << 56)); +} diff --git a/can/common.h b/can/common.h new file mode 100644 index 0000000..848c1bb --- /dev/null +++ b/can/common.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include + +#include "common_dbc.h" +#include +#include "cereal/gen/cpp/log.capnp.h" + +#define MAX_BAD_COUNTER 5 + +// Helper functions +unsigned int honda_checksum(unsigned int address, uint64_t d, int l); +unsigned int toyota_checksum(unsigned int address, uint64_t d, int l); +void init_crc_lookup_tables(); +unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l); +unsigned int pedal_checksum(uint64_t d, int l); +uint64_t read_u64_be(const uint8_t* v); +uint64_t read_u64_le(const uint8_t* v); + +class MessageState { +public: + uint32_t address; + unsigned int size; + + std::vector parse_sigs; + std::vector vals; + + uint16_t ts; + uint64_t seen; + uint64_t check_threshold; + + uint8_t counter; + uint8_t counter_fail; + + bool parse(uint64_t sec, uint16_t ts_, uint8_t * dat); + bool update_counter_generic(int64_t v, int cnt_size); +}; + +class CANParser { +private: + const int bus; + + const DBC *dbc = NULL; + std::unordered_map message_states; + +public: + bool can_valid = false; + uint64_t last_sec = 0; + + 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); + void update_string(std::string data, bool sendcan); + std::vector query_latest(); + +}; diff --git a/can/common.pxd b/can/common.pxd new file mode 100644 index 0000000..366abf4 --- /dev/null +++ b/can/common.pxd @@ -0,0 +1,73 @@ +# distutils: language = c++ +#cython: language_level=3 + +from libc.stdint cimport uint32_t, uint64_t, uint16_t +from libcpp.vector cimport vector +from libcpp.map cimport map +from libcpp.string cimport string +from libcpp.unordered_set cimport unordered_set +from libcpp cimport bool + + +cdef extern from "common.h": + + ctypedef enum SignalType: + DEFAULT, + HONDA_CHECKSUM, + HONDA_COUNTER, + TOYOTA_CHECKSUM, + PEDAL_CHECKSUM, + PEDAL_COUNTER, + VOLKSWAGEN_CHECKSUM, + VOLKSWAGEN_COUNTER + + cdef struct Signal: + const char* name + int b1, b2, bo + bool is_signed + double factor, offset + SignalType type + + cdef struct Msg: + const char* name + uint32_t address + unsigned int size + size_t num_sigs + const Signal *sigs + + cdef struct Val: + const char* name + uint32_t address + const char* def_val + const Signal *sigs + + cdef struct DBC: + const char* name + size_t num_msgs + const Msg *msgs + const Val *vals + size_t num_vals + + cdef struct SignalParseOptions: + uint32_t address + const char* name + double default_value + + + cdef struct MessageParseOptions: + uint32_t address + int check_frequency + + cdef struct SignalValue: + uint32_t address + uint16_t ts + const char* name + double value + + cdef const DBC* dbc_lookup(const string); + + cdef cppclass CANParser: + bool can_valid + CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions]) + void update_string(string, bool) + vector[SignalValue] query_latest() diff --git a/can/common_dbc.h b/can/common_dbc.h new file mode 100644 index 0000000..ae6f443 --- /dev/null +++ b/can/common_dbc.h @@ -0,0 +1,82 @@ +#pragma once + +#include +#include +#include + +#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) + +struct SignalPackValue { + const char* name; + double value; +}; + +struct SignalParseOptions { + uint32_t address; + const char* name; + double default_value; +}; + +struct MessageParseOptions { + uint32_t address; + int check_frequency; +}; + +struct SignalValue { + uint32_t address; + uint16_t ts; + const char* name; + double value; +}; + +enum SignalType { + DEFAULT, + HONDA_CHECKSUM, + HONDA_COUNTER, + TOYOTA_CHECKSUM, + PEDAL_CHECKSUM, + PEDAL_COUNTER, + VOLKSWAGEN_CHECKSUM, + VOLKSWAGEN_COUNTER, +}; + +struct Signal { + const char* name; + int b1, b2, bo; + bool is_signed; + double factor, offset; + bool is_little_endian; + SignalType type; +}; + +struct Msg { + const char* name; + uint32_t address; + unsigned int size; + size_t num_sigs; + const Signal *sigs; +}; + +struct Val { + const char* name; + uint32_t address; + const char* def_val; + const Signal *sigs; +}; + +struct DBC { + const char* name; + size_t num_msgs; + const Msg *msgs; + const Val *vals; + size_t num_vals; +}; + +const DBC* dbc_lookup(const std::string& dbc_name); + +void dbc_register(const DBC* dbc); + +#define dbc_init(dbc) \ +static void __attribute__((constructor)) do_dbc_init_ ## dbc(void) { \ + dbc_register(&dbc); \ +} diff --git a/can/dbc.cc b/can/dbc.cc new file mode 100644 index 0000000..6587de7 --- /dev/null +++ b/can/dbc.cc @@ -0,0 +1,31 @@ +#include + +#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) { + return dbci; + } + } + return NULL; +} + +void dbc_register(const DBC* dbc) { + get_dbcs().push_back(dbc); +} + +extern "C" { + const DBC* dbc_lookup(const char* dbc_name) { + return dbc_lookup(std::string(dbc_name)); + } +} diff --git a/can/dbc.py b/can/dbc.py new file mode 100755 index 0000000..aa52e40 --- /dev/null +++ b/can/dbc.py @@ -0,0 +1,276 @@ +import re +import os +import struct +import sys +import numbers +from collections import namedtuple, defaultdict + +def int_or_float(s): + # return number, trying to maintain int format + if s.isdigit(): + return int(s, 10) + else: + return float(s) + +DBCSignal = namedtuple( + "DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed", + "factor", "offset", "tmin", "tmax", "units"]) + + +class dbc(): + def __init__(self, fn): + self.name, _ = os.path.splitext(os.path.basename(fn)) + with open(fn, encoding="ascii") as f: + self.txt = f.readlines() + self._warned_addresses = set() + + # regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py + bo_regexp = re.compile(r"^BO\_ (\w+) (\w+) *: (\w+) (\w+)") + sg_regexp = re.compile(r"^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)") + sgm_regexp = re.compile(r"^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)") + val_regexp = re.compile(r"VAL\_ (\w+) (\w+) (\s*[-+]?[0-9]+\s+\".+?\"[^;]*)") + + # A dictionary which maps message ids to tuples ((name, size), signals). + # name is the ASCII name of the message. + # size is the size of the message in bytes. + # signals is a list signals contained in the message. + # signals is a list of DBCSignal in order of increasing start_bit. + self.msgs = {} + + # A dictionary which maps message ids to a list of tuples (signal name, definition value pairs) + self.def_vals = defaultdict(list) + + # lookup to bit reverse each byte + self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in range(64)] + + for l in self.txt: + l = l.strip() + + if l.startswith("BO_ "): + # new group + dat = bo_regexp.match(l) + + if dat is None: + print("bad BO {0}".format(l)) + + name = dat.group(2) + size = int(dat.group(3)) + ids = int(dat.group(1), 0) # could be hex + if ids in self.msgs: + sys.exit("Duplicate address detected %d %s" % (ids, self.name)) + + self.msgs[ids] = ((name, size), []) + + if l.startswith("SG_ "): + # new signal + dat = sg_regexp.match(l) + go = 0 + if dat is None: + dat = sgm_regexp.match(l) + go = 1 + + if dat is None: + print("bad SG {0}".format(l)) + + sgname = dat.group(1) + start_bit = int(dat.group(go+2)) + signal_size = int(dat.group(go+3)) + is_little_endian = int(dat.group(go+4))==1 + is_signed = dat.group(go+5)=='-' + factor = int_or_float(dat.group(go+6)) + offset = int_or_float(dat.group(go+7)) + tmin = int_or_float(dat.group(go+8)) + tmax = int_or_float(dat.group(go+9)) + units = dat.group(go+10) + + self.msgs[ids][1].append( + DBCSignal(sgname, start_bit, signal_size, is_little_endian, + is_signed, factor, offset, tmin, tmax, units)) + + if l.startswith("VAL_ "): + # new signal value/definition + dat = val_regexp.match(l) + + if dat is None: + print("bad VAL {0}".format(l)) + + ids = int(dat.group(1), 0) # could be hex + sgname = dat.group(2) + defvals = dat.group(3) + + defvals = defvals.replace("?",r"\?") #escape sequence in C++ + defvals = defvals.split('"')[:-1] + + # convert strings to UPPER_CASE_WITH_UNDERSCORES + defvals[1::2] = [d.strip().upper().replace(" ","_") for d in defvals[1::2]] + defvals = '"'+"".join(str(i) for i in defvals)+'"' + + self.def_vals[ids].append((sgname, defvals)) + + for msg in self.msgs.values(): + msg[1].sort(key=lambda x: x.start_bit) + + self.msg_name_to_address = {} + for address, m in self.msgs.items(): + name = m[0][0] + self.msg_name_to_address[name] = address + + def lookup_msg_id(self, msg_id): + if not isinstance(msg_id, numbers.Number): + msg_id = self.msg_name_to_address[msg_id] + return msg_id + + def reverse_bytes(self, x): + return ((x & 0xff00000000000000) >> 56) | \ + ((x & 0x00ff000000000000) >> 40) | \ + ((x & 0x0000ff0000000000) >> 24) | \ + ((x & 0x000000ff00000000) >> 8) | \ + ((x & 0x00000000ff000000) << 8) | \ + ((x & 0x0000000000ff0000) << 24) | \ + ((x & 0x000000000000ff00) << 40) | \ + ((x & 0x00000000000000ff) << 56) + + def encode(self, msg_id, dd): + """Encode a CAN message using the dbc. + + Inputs: + msg_id: The message ID. + dd: A dictionary mapping signal name to signal data. + """ + msg_id = self.lookup_msg_id(msg_id) + + msg_def = self.msgs[msg_id] + size = msg_def[0][1] + + result = 0 + for s in msg_def[1]: + ival = dd.get(s.name) + if ival is not None: + + ival = (ival / s.factor) - s.offset + ival = int(round(ival)) + + if s.is_signed and ival < 0: + ival = (1 << s.size) + ival + + if s.is_little_endian: + shift = s.start_bit + else: + b1 = (s.start_bit // 8) * 8 + (-s.start_bit - 1) % 8 + shift = 64 - (b1 + s.size) + + mask = ((1 << s.size) - 1) << shift + dat = (ival & ((1 << s.size) - 1)) << shift + + if s.is_little_endian: + mask = self.reverse_bytes(mask) + dat = self.reverse_bytes(dat) + + result &= ~mask + result |= dat + + result = struct.pack('>Q', result) + return result[:size] + + def decode(self, x, arr=None, debug=False): + """Decode a CAN message using the dbc. + + Inputs: + x: A collection with elements (address, time, data), where address is + the CAN address, time is the bus time, and data is the CAN data as a + hex string. + arr: Optional list of signals which should be decoded and returned. + debug: True to print debugging statements. + + Returns: + A tuple (name, data), where name is the name of the CAN message and data + is the decoded result. If arr is None, data is a dict of properties. + Otherwise data is a list of the same length as arr. + + Returns (None, None) if the message could not be decoded. + """ + + if arr is None: + out = {} + else: + out = [None]*len(arr) + + msg = self.msgs.get(x[0]) + if msg is None: + if x[0] not in self._warned_addresses: + #print("WARNING: Unknown message address {}".format(x[0])) + self._warned_addresses.add(x[0]) + return None, None + + name = msg[0][0] + if debug: + print(name) + + st = x[2].ljust(8, b'\x00') + le, be = None, None + + for s in msg[1]: + if arr is not None and s[0] not in arr: + continue + + start_bit = s[1] + signal_size = s[2] + little_endian = s[3] + signed = s[4] + factor = s[5] + offset = s[6] + + if little_endian: + if le is None: + le = struct.unpack("Q", st)[0] + tmp = be + b1 = (start_bit // 8) * 8 + (-start_bit - 1) % 8 + shift_amount = 64 - (b1 + signal_size) + + if shift_amount < 0: + continue + + tmp = (tmp >> shift_amount) & ((1 << signal_size) - 1) + if signed and (tmp >> (signal_size - 1)): + tmp -= (1 << signal_size) + + tmp = tmp * factor + offset + + # if debug: + # print("%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1])) + + if arr is None: + out[s[0]] = tmp + else: + out[arr.index(s[0])] = tmp + return name, out + + def get_signals(self, msg): + msg = self.lookup_msg_id(msg) + return [sgs.name for sgs in self.msgs[msg][1]] + + +if __name__ == "__main__": + from opendbc import DBC_PATH + import numpy as np + + dbc_test = dbc(os.path.join(DBC_PATH, 'toyota_prius_2017_pt_generated.dbc')) + msg = ('STEER_ANGLE_SENSOR', {'STEER_ANGLE': -6.0, 'STEER_RATE': 4, 'STEER_FRACTION': -0.2}) + encoded = dbc_test.encode(*msg) + decoded = dbc_test.decode((0x25, 0, encoded)) + assert decoded == msg + + dbc_test = dbc(os.path.join(DBC_PATH, 'hyundai_santa_fe_2019_ccan.dbc')) + decoded = dbc_test.decode((0x2b0, 0, "\xfa\xfe\x00\x07\x12")) + assert np.isclose(decoded[1]['SAS_Angle'], -26.2) + + msg = ('SAS11', {'SAS_Stat': 7.0, 'MsgCount': 0.0, 'SAS_Angle': -26.200000000000003, 'SAS_Speed': 0.0, 'CheckSum': 0.0}) + encoded = dbc_test.encode(*msg) + decoded = dbc_test.decode((0x2b0, 0, encoded)) + + assert decoded == msg diff --git a/can/dbc_out/.gitignore b/can/dbc_out/.gitignore new file mode 100644 index 0000000..4625581 --- /dev/null +++ b/can/dbc_out/.gitignore @@ -0,0 +1,2 @@ +*.cc + diff --git a/can/dbc_out/.gitkeep b/can/dbc_out/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/can/dbc_template.cc b/can/dbc_template.cc new file mode 100644 index 0000000..f9540fc --- /dev/null +++ b/can/dbc_template.cc @@ -0,0 +1,81 @@ +#include "common_dbc.h" + +namespace { + +{% for address, msg_name, msg_size, sigs in msgs %} +const Signal sigs_{{address}}[] = { + {% for sig in sigs %} + { + {% if sig.is_little_endian %} + {% set b1 = sig.start_bit %} + {% else %} + {% set b1 = (sig.start_bit//8)*8 + (-sig.start_bit-1) % 8 %} + {% endif %} + .name = "{{sig.name}}", + .b1 = {{b1}}, + .b2 = {{sig.size}}, + .bo = {{64 - (b1 + sig.size)}}, + .is_signed = {{"true" if sig.is_signed else "false"}}, + .factor = {{sig.factor}}, + .offset = {{sig.offset}}, + .is_little_endian = {{"true" if sig.is_little_endian else "false"}}, + {% if checksum_type == "honda" and sig.name == "CHECKSUM" %} + .type = SignalType::HONDA_CHECKSUM, + {% elif checksum_type == "honda" and sig.name == "COUNTER" %} + .type = SignalType::HONDA_COUNTER, + {% elif checksum_type == "toyota" and sig.name == "CHECKSUM" %} + .type = SignalType::TOYOTA_CHECKSUM, + {% elif checksum_type == "volkswagen" and sig.name == "CHECKSUM" %} + .type = SignalType::VOLKSWAGEN_CHECKSUM, + {% elif checksum_type == "volkswagen" and sig.name == "COUNTER" %} + .type = SignalType::VOLKSWAGEN_COUNTER, + {% elif address in [512, 513] and sig.name == "CHECKSUM_PEDAL" %} + .type = SignalType::PEDAL_CHECKSUM, + {% elif address in [512, 513] and sig.name == "COUNTER_PEDAL" %} + .type = SignalType::PEDAL_COUNTER, + {% else %} + .type = SignalType::DEFAULT, + {% endif %} + }, + {% endfor %} +}; +{% endfor %} + +const Msg msgs[] = { +{% for address, msg_name, msg_size, sigs in msgs %} + {% set address_hex = "0x%X" % address %} + { + .name = "{{msg_name}}", + .address = {{address_hex}}, + .size = {{msg_size}}, + .num_sigs = ARRAYSIZE(sigs_{{address}}), + .sigs = sigs_{{address}}, + }, +{% endfor %} +}; + +const Val vals[] = { +{% for address, sig in def_vals %} + {% for sg_name, def_val in sig %} + {% set address_hex = "0x%X" % address %} + { + .name = "{{sg_name}}", + .address = {{address_hex}}, + .def_val = {{def_val}}, + .sigs = sigs_{{address}}, + }, + {% endfor %} +{% endfor %} +}; + +} + +const DBC {{dbc.name}} = { + .name = "{{dbc.name}}", + .num_msgs = ARRAYSIZE(msgs), + .msgs = msgs, + .vals = vals, + .num_vals = ARRAYSIZE(vals), +}; + +dbc_init({{dbc.name}}) diff --git a/can/dbc_test.py b/can/dbc_test.py new file mode 100644 index 0000000..339e2a1 --- /dev/null +++ b/can/dbc_test.py @@ -0,0 +1,33 @@ +import sys +import os +import dbcs +from common.dbc import dbc + +def run_test(test): + dbcpath = dbcs.DBC_PATH + if test == "delphi": + dbc_read = dbc(os.path.join(dbcpath, "delphi_esr2_64.dbc")) + st = "0021e7cd0000864d".decode("hex") + msg = 1280 + elif test == "nidec": + dbc_read = dbc(os.path.join(dbcpath, "acura_ilx_2016_nidec.dbc")) + st = "0118111100000000".decode("hex") + msg = 0x301 + elif test == "steer_status": + dbc_read = dbc(os.path.join(dbcpath, "acura_ilx_2016_can.dbc")) + st = "00000000000000".decode("hex") + msg = 0x18f + + print(st) + print('here', dbc_read.msgs[msg]) + out = dbc_read.decode([msg, 0, st], debug=True)[1] + print("out!!",out) + encoded = dbc_read.encode(msg, out) + print("compare") + print(encoded.encode("hex")) + print(st.encode("hex")) + assert encoded == st + +if __name__ == "__main__": + map(run_test, ['delphi', 'nidec', 'steer_status']) + diff --git a/can/libdbc_py.py b/can/libdbc_py.py new file mode 100644 index 0000000..92093f9 --- /dev/null +++ b/can/libdbc_py.py @@ -0,0 +1,95 @@ +import os +import subprocess + +from cffi import FFI + +can_dir = os.path.dirname(os.path.abspath(__file__)) +libdbc_fn = os.path.join(can_dir, "libdbc.so") +subprocess.check_call(["make", "-j3"], cwd=can_dir) # don't use all the cores to avoid overheating + +ffi = FFI() +ffi.cdef(""" + +typedef struct { + const char* name; + double value; +} SignalPackValue; + +typedef struct { + uint32_t address; + const char* name; + double default_value; +} SignalParseOptions; + +typedef struct { + uint32_t address; + int check_frequency; +} MessageParseOptions; + +typedef struct { + uint32_t address; + uint16_t ts; + const char* name; + double value; +} SignalValue; + + +typedef enum { + DEFAULT, + HONDA_CHECKSUM, + HONDA_COUNTER, + TOYOTA_CHECKSUM, + PEDAL_CHECKSUM, + PEDAL_COUNTER, + VOLKSWAGEN_CHECKSUM, + VOLKSWAGEN_COUNTER, +} SignalType; + +typedef struct { + const char* name; + int b1, b2, bo; + bool is_signed; + double factor, offset; + SignalType type; +} Signal; + +typedef struct { + const char* name; + uint32_t address; + unsigned int size; + size_t num_sigs; + const Signal *sigs; +} Msg; + +typedef struct { + const char* name; + uint32_t address; + const char* def_val; + const Signal *sigs; +} Val; + +typedef struct { + const char* name; + size_t num_msgs; + const Msg *msgs; + const Val *vals; + size_t num_vals; +} DBC; + + +void* can_init(int bus, const char* dbc_name, + size_t num_message_options, const MessageParseOptions* message_options, + size_t num_signal_options, const SignalParseOptions* signal_options); + +void can_update_string(void *can, const char* dat, int len); + +size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values); + +const DBC* dbc_lookup(const char* dbc_name); + +void* canpack_init(const char* dbc_name); + +uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals, int counter); +""") + +libdbc = ffi.dlopen(libdbc_fn) diff --git a/can/packer.cc b/can/packer.cc new file mode 100644 index 0000000..c658e56 --- /dev/null +++ b/can/packer.cc @@ -0,0 +1,139 @@ +#include +#include +#include +#include +#include + +#include "common.h" + +#define WARN printf + +namespace { + + // this is the same as read_u64_le, but uses uint64_t as in/out + uint64_t ReverseBytes(uint64_t x) { + return ((x & 0xff00000000000000ull) >> 56) | + ((x & 0x00ff000000000000ull) >> 40) | + ((x & 0x0000ff0000000000ull) >> 24) | + ((x & 0x000000ff00000000ull) >> 8) | + ((x & 0x00000000ff000000ull) << 8) | + ((x & 0x0000000000ff0000ull) << 24) | + ((x & 0x000000000000ff00ull) << 40) | + ((x & 0x00000000000000ffull) << 56); + } + + uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){ + int shift = sig.is_little_endian? sig.b1 : sig.bo; + uint64_t mask = ((1ULL << sig.b2)-1) << shift; + uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift; + if (sig.is_little_endian) { + dat = ReverseBytes(dat); + mask = ReverseBytes(mask); + } + ret &= ~mask; + ret |= dat; + return ret; + } + + class CANPacker { + public: + CANPacker(const std::string& dbc_name) { + dbc = dbc_lookup(dbc_name); + assert(dbc); + + for (int i=0; inum_msgs; i++) { + const Msg* msg = &dbc->msgs[i]; + message_lookup[msg->address] = *msg; + for (int j=0; jnum_sigs; j++) { + const Signal* sig = &msg->sigs[j]; + signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig; + } + } + init_crc_lookup_tables(); + } + + uint64_t pack(uint32_t address, const std::vector &signals, int counter) { + uint64_t ret = 0; + for (const auto& sigval : signals) { + std::string name = std::string(sigval.name); + double value = sigval.value; + + auto sig_it = signal_lookup.find(std::make_pair(address, name)); + if (sig_it == signal_lookup.end()) { + WARN("undefined signal %s - %d\n", name.c_str(), address); + continue; + } + auto sig = sig_it->second; + + int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor)); + if (ival < 0) { + ival = (1ULL << sig.b2) + ival; + } + + ret = set_value(ret, sig, ival); + } + + if (counter >= 0){ + auto sig_it = signal_lookup.find(std::make_pair(address, "COUNTER")); + if (sig_it == signal_lookup.end()) { + WARN("COUNTER not defined\n"); + return ret; + } + auto sig = sig_it->second; + + if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) { + WARN("COUNTER signal type not valid\n"); + } + + ret = set_value(ret, sig, counter); + } + + auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM")); + if (sig_it_checksum != signal_lookup.end()) { + auto sig = sig_it_checksum->second; + if (sig.type == SignalType::HONDA_CHECKSUM) { + unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size); + ret = set_value(ret, sig, chksm); + } else if (sig.type == SignalType::TOYOTA_CHECKSUM) { + unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size); + ret = set_value(ret, sig, chksm); + } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) { + // FIXME: Hackish fix for an endianness issue. The message is in reverse byte order + // until later in the pack process. Checksums can be run backwards, CRCs not so much. + // The correct fix is unclear but this works for the moment. + unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size); + ret = set_value(ret, sig, chksm); + } else { + //WARN("CHECKSUM signal type not valid\n"); + } + } + + return ret; + } + + + private: + const DBC *dbc = NULL; + std::map, Signal> signal_lookup; + std::map message_lookup; + }; + +} + +extern "C" { + void* canpack_init(const char* dbc_name) { + CANPacker *ret = new CANPacker(std::string(dbc_name)); + return (void*)ret; + } + + uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals, int counter, bool checksum) { + CANPacker *cp = (CANPacker*)inst; + + return cp->pack(address, std::vector(vals, vals+num_vals), counter); + } + + uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector &signals, int counter) { + CANPacker *cp = (CANPacker*)inst; + return cp->pack(address, signals, counter); + } +} diff --git a/can/packer.py b/can/packer.py new file mode 100644 index 0000000..95a4e13 --- /dev/null +++ b/can/packer.py @@ -0,0 +1,9 @@ +# pylint: skip-file +import os +import subprocess + +can_dir = os.path.dirname(os.path.abspath(__file__)) +subprocess.check_call(["make", "-j3", "packer_impl.so"], cwd=can_dir) # don't use all the cores to avoid overheating + +from opendbc.can.packer_impl import CANPacker +assert CANPacker diff --git a/can/packer_impl.pyx b/can/packer_impl.pyx new file mode 100644 index 0000000..d7c6119 --- /dev/null +++ b/can/packer_impl.pyx @@ -0,0 +1,124 @@ +# distutils: language = c++ +# cython: c_string_encoding=ascii, language_level=3 + +from libc.stdint cimport uint32_t, uint64_t +from libcpp.vector cimport vector +from libcpp.map cimport map +from libcpp.string cimport string +from libcpp cimport bool +from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY +import os +import subprocess + +cdef struct SignalPackValue: + const char* name + double value + +ctypedef enum SignalType: + DEFAULT, + HONDA_CHECKSUM, + HONDA_COUNTER, + TOYOTA_CHECKSUM, + PEDAL_CHECKSUM, + PEDAL_COUNTER, + VOLKSWAGEN_CHECKSUM, + VOLKSWAGEN_COUNTER + +cdef struct Signal: + const char* name + int b1, b2, bo + bool is_signed + double factor, offset + SignalType type + + + +cdef struct Msg: + const char* name + uint32_t address + unsigned int size + size_t num_sigs + const Signal *sigs + +cdef struct Val: + const char* name + uint32_t address + const char* def_val + const Signal *sigs + +cdef struct DBC: + const char* name + size_t num_msgs + const Msg *msgs + const Val *vals + size_t num_vals + +ctypedef void * (*canpack_init_func)(const char* dbc_name) +ctypedef uint64_t (*canpack_pack_vector_func)(void* inst, uint32_t address, const vector[SignalPackValue] &signals, int counter) +ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name) + + +cdef class CANPacker(): + cdef void *packer + cdef const DBC *dbc + cdef map[string, (int, int)] name_to_address_and_size + cdef map[int, int] address_to_size + cdef canpack_init_func canpack_init + cdef canpack_pack_vector_func canpack_pack_vector + cdef dbc_lookup_func dbc_lookup + + def __init__(self, dbc_name): + can_dir = os.path.dirname(os.path.abspath(__file__)) + libdbc_fn = os.path.join(can_dir, "libdbc.so") + libdbc_fn = str(libdbc_fn).encode('utf8') + subprocess.check_call(["make"], cwd=can_dir) + + cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY) + self.canpack_init = dlsym(libdbc, 'canpack_init') + self.canpack_pack_vector = dlsym(libdbc, 'canpack_pack_vector') + self.dbc_lookup = dlsym(libdbc, 'dbc_lookup') + + self.packer = self.canpack_init(dbc_name) + self.dbc = self.dbc_lookup(dbc_name) + num_msgs = self.dbc[0].num_msgs + for i in range(num_msgs): + msg = self.dbc[0].msgs[i] + self.name_to_address_and_size[string(msg.name)] = (msg.address, msg.size) + self.address_to_size[msg.address] = msg.size + + cdef uint64_t pack(self, addr, values, counter): + cdef vector[SignalPackValue] values_thing + cdef SignalPackValue spv + + names = [] + + for name, value in values.iteritems(): + n = name.encode('utf8') + names.append(n) # TODO: find better way to keep reference to temp string arround + + spv.name = n + spv.value = value + values_thing.push_back(spv) + + return self.canpack_pack_vector(self.packer, addr, values_thing, counter) + + cdef inline uint64_t ReverseBytes(self, uint64_t x): + return (((x & 0xff00000000000000ull) >> 56) | + ((x & 0x00ff000000000000ull) >> 40) | + ((x & 0x0000ff0000000000ull) >> 24) | + ((x & 0x000000ff00000000ull) >> 8) | + ((x & 0x00000000ff000000ull) << 8) | + ((x & 0x0000000000ff0000ull) << 24) | + ((x & 0x000000000000ff00ull) << 40) | + ((x & 0x00000000000000ffull) << 56)) + + cpdef make_can_msg(self, name_or_addr, bus, values, counter=-1): + cdef int addr, size + if type(name_or_addr) == int: + addr = name_or_addr + size = self.address_to_size[name_or_addr] + else: + addr, size = self.name_to_address_and_size[name_or_addr.encode('utf8')] + cdef uint64_t val = self.pack(addr, values, counter) + val = self.ReverseBytes(val) + return [addr, 0, (&val)[:size], bus] diff --git a/can/packer_setup.py b/can/packer_setup.py new file mode 100644 index 0000000..8b25e60 --- /dev/null +++ b/can/packer_setup.py @@ -0,0 +1,9 @@ +from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module + +from Cython.Build import cythonize + +from common.cython_hacks import BuildExtWithoutPlatformSuffix + +setup(name='CAN Packer API Implementation', + cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, + ext_modules=cythonize(Extension("packer_impl", ["packer_impl.pyx"], language="c++", extra_compile_args=["-std=c++11"]))) diff --git a/can/parser.cc b/can/parser.cc new file mode 100644 index 0000000..69fe374 --- /dev/null +++ b/can/parser.cc @@ -0,0 +1,239 @@ +#include +#include + +#include +#include +#include +#include +#include + +#include "common.h" + +#define DEBUG(...) +// #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); + + for (int i=0; i < parse_sigs.size(); i++) { + auto& sig = parse_sigs[i]; + int64_t tmp; + + if (sig.is_little_endian){ + tmp = (dat_le >> sig.b1) & ((1ULL << sig.b2)-1); + } else { + tmp = (dat_be >> sig.bo) & ((1ULL << sig.b2)-1); + } + + if (sig.is_signed) { + tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed + } + + 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; + } + } 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 (!update_counter_generic(tmp, sig.b2)) { + 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; + } + } + + vals[i] = tmp * sig.factor + sig.offset; + } + ts = ts_; + seen = sec; + + return true; +} + + +bool MessageState::update_counter_generic(int64_t v, int cnt_size) { + uint8_t old_counter = counter; + counter = v; + if (((old_counter+1) & ((1 << cnt_size) -1)) != v) { + counter_fail += 1; + if (counter_fail > 1) { + INFO("0x%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v); + } + if (counter_fail >= MAX_BAD_COUNTER) { + return false; + } + } else if (counter_fail > 0) { + counter_fail--; + } + return true; +} + + +CANParser::CANParser(int abus, const std::string& dbc_name, + const std::vector &options, + const std::vector &sigoptions) + : bus(abus) { + + dbc = dbc_lookup(dbc_name); + assert(dbc); + init_crc_lookup_tables(); + + for (const auto& op : options) { + MessageState state = { + .address = op.address, + // .check_frequency = op.check_frequency, + }; + + // msg is not valid if a message isn't received for 10 consecutive steps + if (op.check_frequency > 0) { + state.check_threshold = (1000000000ULL / op.check_frequency) * 10; + } + + + const Msg* msg = NULL; + for (int i=0; inum_msgs; i++) { + if (dbc->msgs[i].address == op.address) { + msg = &dbc->msgs[i]; + break; + } + } + if (!msg) { + fprintf(stderr, "CANParser: could not find message 0x%X in DBC %s\n", op.address, dbc_name.c_str()); + assert(false); + } + + state.size = msg->size; + + // track checksums and counters for this message + for (int i=0; inum_sigs; i++) { + const Signal *sig = &msg->sigs[i]; + if (sig->type != SignalType::DEFAULT) { + state.parse_sigs.push_back(*sig); + state.vals.push_back(0); + } + } + + // track requested signals for this message + for (const auto& sigop : sigoptions) { + if (sigop.address != op.address) continue; + + for (int i=0; inum_sigs; i++) { + const Signal *sig = &msg->sigs[i]; + if (strcmp(sig->name, sigop.name) == 0 + && sig->type == SignalType::DEFAULT) { + state.parse_sigs.push_back(*sig); + state.vals.push_back(sigop.default_value); + break; + } + } + + } + + message_states[state.address] = state; + } +} + +void CANParser::UpdateCans(uint64_t sec, const capnp::List::Reader& cans) { + int msg_count = cans.size(); + uint64_t p; + + DEBUG("got %d messages\n", msg_count); + + // 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; + } + + if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen + uint8_t dat[8] = {0}; + memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); + + state_it->second.parse(sec, cmsg.getBusTime(), dat); + } +} + +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; + } + } +} + +void CANParser::update_string(std::string data, bool sendcan) { + // format for board, make copy due to alignment issues, will be freed on out of scope + auto amsg = kj::heapArray((data.length() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), data.data(), data.length()); + + // extract the messages + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + last_sec = event.getLogMonoTime(); + + auto cans = sendcan? event.getSendcan() : event.getCan(); + UpdateCans(last_sec, cans); + + UpdateValid(last_sec); +} + + +std::vector CANParser::query_latest() { + std::vector ret; + + for (const auto& kv : message_states) { + const auto& state = kv.second; + if (last_sec != 0 && state.seen != last_sec) continue; + + for (int i=0; iself.address_to_msg_name[cv.address].c_str() + cv_name = cv.name + + self.vl[cv.address][cv_name] = cv.value + self.ts[cv.address][cv_name] = cv.ts + + self.vl[name][cv_name] = cv.value + self.ts[name][cv_name] = cv.ts + + updated_val.insert(cv.address) + + return updated_val + + def update_string(self, dat, sendcan=False): + self.can.update_string(dat, sendcan) + return self.update_vl() + + def update_strings(self, strings, sendcan=False): + updated_vals = set() + + for s in strings: + updated_val = self.update_string(s, sendcan) + updated_vals.update(updated_val) + + return updated_vals diff --git a/can/parser_pyx_setup.py b/can/parser_pyx_setup.py new file mode 100644 index 0000000..b9a1117 --- /dev/null +++ b/can/parser_pyx_setup.py @@ -0,0 +1,35 @@ +import os +import subprocess +from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module + +from Cython.Build import cythonize + +from common.basedir import BASEDIR +from common.cython_hacks import BuildExtWithoutPlatformSuffix + +sourcefiles = ['parser_pyx.pyx'] +extra_compile_args = ["-std=c++11"] +ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg + +if ARCH == "aarch64": + extra_compile_args += ["-Wno-deprecated-register"] + +setup(name='CAN parser', + cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, + ext_modules=cythonize( + Extension( + "parser_pyx", + language="c++", + sources=sourcefiles, + extra_compile_args=extra_compile_args, + include_dirs=[ + BASEDIR, + os.path.join(BASEDIR, 'phonelibs', 'capnp-cpp/include'), + ], + extra_link_args=[ + os.path.join(BASEDIR, 'opendbc', 'can', 'libdbc.so'), + ], + ) + ), + nthreads=4, +) diff --git a/can/process_dbc.py b/can/process_dbc.py new file mode 100755 index 0000000..be6d827 --- /dev/null +++ b/can/process_dbc.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +from __future__ import print_function +import os +import sys + +import jinja2 + +from collections import Counter +from opendbc.can.dbc import dbc + +def main(): + if len(sys.argv) != 3: + print("usage: %s dbc_directory output_filename" % (sys.argv[0],)) + sys.exit(0) + + dbc_dir = sys.argv[1] + out_fn = sys.argv[2] + + dbc_name = os.path.split(out_fn)[-1].replace('.cc', '') + + template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc") + + with open(template_fn, "r") as template_f: + template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True) + + can_dbc = dbc(os.path.join(dbc_dir, dbc_name + '.dbc')) + + msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first + for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs] + + def_vals = {a: sorted(set(b)) for a, b in can_dbc.def_vals.items()} # remove duplicates + def_vals = sorted(def_vals.items()) + + if can_dbc.name.startswith(("honda_", "acura_")): + checksum_type = "honda" + checksum_size = 4 + counter_size = 2 + checksum_start_bit = 3 + counter_start_bit = 5 + little_endian = False + elif can_dbc.name.startswith(("toyota_", "lexus_")): + checksum_type = "toyota" + checksum_size = 8 + counter_size = None + checksum_start_bit = 7 + counter_start_bit = None + little_endian = False + elif can_dbc.name.startswith(("vw_", "volkswagen_", "audi_", "seat_", "skoda_")): + checksum_type = "volkswagen" + checksum_size = 8 + counter_size = 4 + checksum_start_bit = 0 + counter_start_bit = 0 + little_endian = True + else: + checksum_type = None + checksum_size = None + counter_size = None + checksum_start_bit = None + counter_start_bit = None + little_endian = None + + # sanity checks on expected COUNTER and CHECKSUM rules, as packer and parser auto-compute those signals + for address, msg_name, msg_size, sigs in msgs: + dbc_msg_name = dbc_name + " " + msg_name + for sig in sigs: + if checksum_type is not None: + # checksum rules + if sig.name == "CHECKSUM": + if sig.size != checksum_size: + sys.exit("%s: CHECKSUM is not %d bits long" % (dbc_msg_name, checksum_size)) + if sig.start_bit % 8 != checksum_start_bit: + sys.exit("%s: CHECKSUM starts at wrong bit" % dbc_msg_name) + if little_endian != sig.is_little_endian: + sys.exit("%s: CHECKSUM has wrong endianess" % dbc_msg_name) + # counter rules + if sig.name == "COUNTER": + if counter_size is not None and sig.size != counter_size: + sys.exit("%s: COUNTER is not %d bits long" % (dbc_msg_name, counter_size)) + if counter_start_bit is not None and sig.start_bit % 8 != counter_start_bit: + print(counter_start_bit, sig.start_bit) + sys.exit("%s: COUNTER starts at wrong bit" % dbc_msg_name) + if little_endian != sig.is_little_endian: + sys.exit("%s: COUNTER has wrong endianess" % dbc_msg_name) + # pedal rules + if address in [0x200, 0x201]: + if sig.name == "COUNTER_PEDAL" and sig.size != 4: + sys.exit("%s: PEDAL COUNTER is not 4 bits long" % dbc_msg_name) + if sig.name == "CHECKSUM_PEDAL" and sig.size != 8: + sys.exit("%s: PEDAL CHECKSUM is not 8 bits long" % dbc_msg_name) + + # Fail on duplicate message names + c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs]) + for name, count in c.items(): + if count > 1: + sys.exit("%s: Duplicate message name in DBC file %s" % (dbc_name, name)) + + 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) + +if __name__ == '__main__': + main() diff --git a/can/tests/.gitignore b/can/tests/.gitignore new file mode 100644 index 0000000..192fb09 --- /dev/null +++ b/can/tests/.gitignore @@ -0,0 +1 @@ +*.bz2 diff --git a/can/tests/__init__.py b/can/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/can/tests/test_packer_parser.py b/can/tests/test_packer_parser.py new file mode 100644 index 0000000..d725776 --- /dev/null +++ b/can/tests/test_packer_parser.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +import unittest + +from opendbc.can.parser import CANParser +from opendbc.can.packer import CANPacker +from opendbc.boardd.boardd import can_list_to_can_capnp + +class TestCanParserPacker(unittest.TestCase): + def test_civic(self): + + dbc_file = "honda_civic_touring_2016_can_generated" + + signals = [ + ("STEER_TORQUE", "STEERING_CONTROL", 0), + ("STEER_TORQUE_REQUEST", "STEERING_CONTROL", 0), + ] + checks = [] + + parser = CANParser(dbc_file, signals, checks, 0) + packer = CANPacker(dbc_file) + + idx = 0 + + for steer in range(-256, 255): + for active in [1, 0]: + values = { + "STEER_TORQUE": steer, + "STEER_TORQUE_REQUEST": active, + } + + msgs = packer.make_can_msg("STEERING_CONTROL", 0, values, idx) + bts = can_list_to_can_capnp([msgs]) + + parser.update_string(bts) + + self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE"], steer) + self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE_REQUEST"], active) + self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["COUNTER"], idx % 4) + + idx += 1 + + def test_subaru(self): + # Subuaru is little endian + + dbc_file = "subaru_global_2017" + + signals = [ + ("Counter", "ES_LKAS", 0), + ("LKAS_Output", "ES_LKAS", 0), + ("LKAS_Request", "ES_LKAS", 0), + ("SET_1", "ES_LKAS", 0), + + ] + + checks = [] + + parser = CANParser(dbc_file, signals, checks, 0) + packer = CANPacker(dbc_file) + + idx = 0 + + for steer in range(-256, 255): + for active in [1, 0]: + values = { + "Counter": idx, + "LKAS_Output": steer, + "LKAS_Request": active, + "SET_1": 1 + } + + msgs = packer.make_can_msg("ES_LKAS", 0, values) + bts = can_list_to_can_capnp([msgs]) + parser.update_string(bts) + + self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Output"], steer) + self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Request"], active) + self.assertAlmostEqual(parser.vl["ES_LKAS"]["SET_1"], 1) + self.assertAlmostEqual(parser.vl["ES_LKAS"]["Counter"], idx % 16) + + idx += 1 + + +if __name__ == "__main__": + unittest.main()