unify can packer and parser
Willem Melching
parent
25d88009b6
commit
0ba7926bfe
13
can/common.h
13
can/common.h
|
|
@ -1,6 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "common_dbc.h"
|
||||
|
|
@ -55,5 +56,15 @@ public:
|
|||
void UpdateValid(uint64_t sec);
|
||||
void update_string(std::string data, bool sendcan);
|
||||
std::vector<SignalValue> query_latest();
|
||||
|
||||
};
|
||||
|
||||
class CANPacker {
|
||||
private:
|
||||
const DBC *dbc = NULL;
|
||||
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
|
||||
std::map<uint32_t, Msg> message_lookup;
|
||||
|
||||
public:
|
||||
CANPacker(const std::string& dbc_name);
|
||||
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -9,8 +9,7 @@ from libcpp.unordered_set cimport unordered_set
|
|||
from libcpp cimport bool
|
||||
|
||||
|
||||
cdef extern from "common.h":
|
||||
|
||||
cdef extern from "common_dbc.h":
|
||||
ctypedef enum SignalType:
|
||||
DEFAULT,
|
||||
HONDA_CHECKSUM,
|
||||
|
|
@ -64,6 +63,12 @@ cdef extern from "common.h":
|
|||
const char* name
|
||||
double value
|
||||
|
||||
cdef struct SignalPackValue:
|
||||
const char * name
|
||||
double value
|
||||
|
||||
|
||||
cdef extern from "common.h":
|
||||
cdef const DBC* dbc_lookup(const string);
|
||||
|
||||
cdef cppclass CANParser:
|
||||
|
|
@ -71,3 +76,7 @@ cdef extern from "common.h":
|
|||
CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions])
|
||||
void update_string(string, bool)
|
||||
vector[SignalValue] query_latest()
|
||||
|
||||
cdef cppclass CANPacker:
|
||||
CANPacker(string)
|
||||
uint64_t pack(uint32_t, vector[SignalPackValue], int counter)
|
||||
|
|
|
|||
195
can/packer.cc
195
can/packer.cc
|
|
@ -8,132 +8,101 @@
|
|||
|
||||
#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);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
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);
|
||||
CANPacker::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;
|
||||
}
|
||||
ret &= ~mask;
|
||||
ret |= dat;
|
||||
return ret;
|
||||
}
|
||||
init_crc_lookup_tables();
|
||||
}
|
||||
|
||||
class CANPacker {
|
||||
public:
|
||||
CANPacker(const std::string& dbc_name) {
|
||||
dbc = dbc_lookup(dbc_name);
|
||||
assert(dbc);
|
||||
uint64_t CANPacker::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;
|
||||
|
||||
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();
|
||||
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;
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
}
|
||||
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");
|
||||
}
|
||||
|
||||
private:
|
||||
const DBC *dbc = NULL;
|
||||
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
|
||||
std::map<uint32_t, Msg> message_lookup;
|
||||
};
|
||||
ret = set_value(ret, sig, counter);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,78 +7,22 @@ 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
|
||||
from common cimport CANPacker as cpp_CANPacker
|
||||
from common cimport dbc_lookup, SignalPackValue, DBC
|
||||
|
||||
|
||||
|
||||
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
|
||||
cdef class CANPacker:
|
||||
cdef:
|
||||
cpp_CANPacker *packer
|
||||
const DBC *dbc
|
||||
map[string, (int, int)] name_to_address_and_size
|
||||
map[int, int] address_to_size
|
||||
|
||||
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')
|
||||
self.packer = new cpp_CANPacker(dbc_name)
|
||||
self.dbc = dbc_lookup(dbc_name)
|
||||
|
||||
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]
|
||||
|
|
@ -99,7 +43,7 @@ cdef class CANPacker():
|
|||
spv.value = value
|
||||
values_thing.push_back(spv)
|
||||
|
||||
return self.canpack_pack_vector(self.packer, addr, values_thing, counter)
|
||||
return self.packer.pack(addr, values_thing, counter)
|
||||
|
||||
cdef inline uint64_t ReverseBytes(self, uint64_t x):
|
||||
return (((x & 0xff00000000000000ull) >> 56) |
|
||||
|
|
|
|||
|
|
@ -1,9 +1,36 @@
|
|||
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
|
||||
|
||||
setup(name='CAN Packer API Implementation',
|
||||
sourcefiles = ['packer_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 packer',
|
||||
cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
|
||||
ext_modules=cythonize(Extension("packer_pyx", ["packer_pyx.pyx"], language="c++", extra_compile_args=["-std=c++11"])))
|
||||
ext_modules=cythonize(
|
||||
Extension(
|
||||
"packer_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,
|
||||
)
|
||||
|
|
|
|||
Loading…
Reference in New Issue