rbiasini
GitHub
parent
3eded83a1f
commit
e155e01748
|
|
@ -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)/*
|
||||
|
|
@ -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]
|
||||
|
|
@ -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 '$@' '$<'
|
||||
|
|
@ -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));
|
||||
}
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "common_dbc.h"
|
||||
#include <capnp/serialize.h>
|
||||
#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<Signal> parse_sigs;
|
||||
std::vector<double> 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<uint32_t, MessageState> message_states;
|
||||
|
||||
public:
|
||||
bool can_valid = false;
|
||||
uint64_t last_sec = 0;
|
||||
|
||||
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);
|
||||
void update_string(std::string data, bool sendcan);
|
||||
std::vector<SignalValue> query_latest();
|
||||
|
||||
};
|
||||
|
|
@ -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()
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
#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); \
|
||||
}
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
#include <vector>
|
||||
|
||||
#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) {
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
|
@ -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 = le
|
||||
shift_amount = start_bit
|
||||
else:
|
||||
if be is None:
|
||||
be = 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
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
*.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}})
|
||||
|
|
@ -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'])
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -0,0 +1,139 @@
|
|||
#include <cassert>
|
||||
#include <utility>
|
||||
#include <algorithm>
|
||||
#include <map>
|
||||
#include <cmath>
|
||||
|
||||
#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; i<dbc->num_msgs; i++) {
|
||||
const Msg* msg = &dbc->msgs[i];
|
||||
message_lookup[msg->address] = *msg;
|
||||
for (int j=0; j<msg->num_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<SignalPackValue> &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<std::pair<uint32_t, std::string>, Signal> signal_lookup;
|
||||
std::map<uint32_t, Msg> 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<SignalPackValue>(vals, vals+num_vals), counter);
|
||||
}
|
||||
|
||||
uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
|
||||
CANPacker *cp = (CANPacker*)inst;
|
||||
return cp->pack(address, signals, counter);
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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 = <canpack_init_func>dlsym(libdbc, 'canpack_init')
|
||||
self.canpack_pack_vector = <canpack_pack_vector_func>dlsym(libdbc, 'canpack_pack_vector')
|
||||
self.dbc_lookup = <dbc_lookup_func>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, (<char *>&val)[:size], bus]
|
||||
|
|
@ -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"])))
|
||||
|
|
@ -0,0 +1,239 @@
|
|||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/mman.h>
|
||||
#include <algorithm>
|
||||
|
||||
#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<MessageParseOptions> &options,
|
||||
const std::vector<SignalParseOptions> &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; i<dbc->num_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; i<msg->num_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; i<msg->num_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<cereal::CanData>::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<capnp::word>((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<cereal::Event>();
|
||||
|
||||
last_sec = event.getLogMonoTime();
|
||||
|
||||
auto cans = sendcan? event.getSendcan() : event.getCan();
|
||||
UpdateCans(last_sec, cans);
|
||||
|
||||
UpdateValid(last_sec);
|
||||
}
|
||||
|
||||
|
||||
std::vector<SignalValue> CANParser::query_latest() {
|
||||
std::vector<SignalValue> 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; i<state.parse_sigs.size(); i++) {
|
||||
const Signal &sig = state.parse_sigs[i];
|
||||
ret.push_back((SignalValue){
|
||||
.address = state.address,
|
||||
.ts = state.ts,
|
||||
.name = sig.name,
|
||||
.value = state.vals[i],
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
import os
|
||||
import subprocess
|
||||
|
||||
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
|
||||
|
||||
from opendbc.can.parser_pyx import CANParser # pylint: disable=no-name-in-module, import-error
|
||||
assert CANParser
|
||||
|
|
@ -0,0 +1,138 @@
|
|||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
from libcpp.string cimport string
|
||||
from libcpp.vector cimport vector
|
||||
from libcpp cimport bool
|
||||
from libcpp.unordered_set cimport unordered_set
|
||||
from libc.stdint cimport uint32_t, uint64_t, uint16_t
|
||||
from libcpp.map cimport map
|
||||
|
||||
from common cimport CANParser as cpp_CANParser
|
||||
from common cimport SignalParseOptions, MessageParseOptions, dbc_lookup, SignalValue, DBC
|
||||
|
||||
|
||||
from libcpp cimport bool
|
||||
import os
|
||||
import numbers
|
||||
|
||||
cdef int CAN_INVALID_CNT = 5
|
||||
|
||||
|
||||
cdef class CANParser:
|
||||
cdef:
|
||||
cpp_CANParser *can
|
||||
const DBC *dbc
|
||||
map[string, uint32_t] msg_name_to_address
|
||||
map[uint32_t, string] address_to_msg_name
|
||||
vector[SignalValue] can_values
|
||||
bool test_mode_enabled
|
||||
|
||||
cdef public:
|
||||
string dbc_name
|
||||
dict vl
|
||||
dict ts
|
||||
bool can_valid
|
||||
int can_invalid_cnt
|
||||
|
||||
def __init__(self, dbc_name, signals, checks=None, bus=0):
|
||||
if checks is None:
|
||||
checks = []
|
||||
|
||||
self.can_valid = True
|
||||
self.dbc_name = dbc_name
|
||||
self.dbc = dbc_lookup(dbc_name)
|
||||
self.vl = {}
|
||||
self.ts = {}
|
||||
|
||||
self.can_invalid_cnt = CAN_INVALID_CNT
|
||||
|
||||
num_msgs = self.dbc[0].num_msgs
|
||||
for i in range(num_msgs):
|
||||
msg = self.dbc[0].msgs[i]
|
||||
name = msg.name.decode('utf8')
|
||||
|
||||
self.msg_name_to_address[name] = msg.address
|
||||
self.address_to_msg_name[msg.address] = name
|
||||
self.vl[msg.address] = {}
|
||||
self.vl[name] = {}
|
||||
self.ts[msg.address] = {}
|
||||
self.ts[name] = {}
|
||||
|
||||
# Convert message names into addresses
|
||||
for i in range(len(signals)):
|
||||
s = signals[i]
|
||||
if not isinstance(s[1], numbers.Number):
|
||||
name = s[1].encode('utf8')
|
||||
s = (s[0], self.msg_name_to_address[name], s[2])
|
||||
signals[i] = s
|
||||
|
||||
for i in range(len(checks)):
|
||||
c = checks[i]
|
||||
if not isinstance(c[0], numbers.Number):
|
||||
name = c[0].encode('utf8')
|
||||
c = (self.msg_name_to_address[name], c[1])
|
||||
checks[i] = c
|
||||
|
||||
cdef vector[SignalParseOptions] signal_options_v
|
||||
cdef SignalParseOptions spo
|
||||
for sig_name, sig_address, sig_default in signals:
|
||||
spo.address = sig_address
|
||||
spo.name = sig_name
|
||||
spo.default_value = sig_default
|
||||
signal_options_v.push_back(spo)
|
||||
|
||||
message_options = dict((address, 0) for _, address, _ in signals)
|
||||
message_options.update(dict(checks))
|
||||
|
||||
cdef vector[MessageParseOptions] message_options_v
|
||||
cdef MessageParseOptions mpo
|
||||
for msg_address, freq in message_options.items():
|
||||
mpo.address = msg_address
|
||||
mpo.check_frequency = freq
|
||||
message_options_v.push_back(mpo)
|
||||
|
||||
self.can = new cpp_CANParser(bus, dbc_name, message_options_v, signal_options_v)
|
||||
self.update_vl()
|
||||
|
||||
cdef unordered_set[uint32_t] update_vl(self):
|
||||
cdef string sig_name
|
||||
cdef unordered_set[uint32_t] updated_val
|
||||
|
||||
can_values = self.can.query_latest()
|
||||
valid = self.can.can_valid
|
||||
|
||||
# Update invalid flag
|
||||
self.can_invalid_cnt += 1
|
||||
if valid:
|
||||
self.can_invalid_cnt = 0
|
||||
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT
|
||||
|
||||
|
||||
for cv in can_values:
|
||||
# Cast char * directly to unicde
|
||||
name = <unicode>self.address_to_msg_name[cv.address].c_str()
|
||||
cv_name = <unicode>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
|
||||
|
|
@ -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,
|
||||
)
|
||||
|
|
@ -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()
|
||||
|
|
@ -0,0 +1 @@
|
|||
*.bz2
|
||||
|
|
@ -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()
|
||||
Loading…
Reference in New Issue