mirror of https://github.com/rusefi/opendbc.git
unify can packer and parser
This commit is contained in:
parent
25d88009b6
commit
0ba7926bfe
13
can/common.h
13
can/common.h
|
@ -1,6 +1,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
#include <map>
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
|
|
||||||
#include "common_dbc.h"
|
#include "common_dbc.h"
|
||||||
|
@ -55,5 +56,15 @@ public:
|
||||||
void UpdateValid(uint64_t sec);
|
void UpdateValid(uint64_t sec);
|
||||||
void update_string(std::string data, bool sendcan);
|
void update_string(std::string data, bool sendcan);
|
||||||
std::vector<SignalValue> query_latest();
|
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
|
from libcpp cimport bool
|
||||||
|
|
||||||
|
|
||||||
cdef extern from "common.h":
|
cdef extern from "common_dbc.h":
|
||||||
|
|
||||||
ctypedef enum SignalType:
|
ctypedef enum SignalType:
|
||||||
DEFAULT,
|
DEFAULT,
|
||||||
HONDA_CHECKSUM,
|
HONDA_CHECKSUM,
|
||||||
|
@ -64,6 +63,12 @@ cdef extern from "common.h":
|
||||||
const char* name
|
const char* name
|
||||||
double value
|
double value
|
||||||
|
|
||||||
|
cdef struct SignalPackValue:
|
||||||
|
const char * name
|
||||||
|
double value
|
||||||
|
|
||||||
|
|
||||||
|
cdef extern from "common.h":
|
||||||
cdef const DBC* dbc_lookup(const string);
|
cdef const DBC* dbc_lookup(const string);
|
||||||
|
|
||||||
cdef cppclass CANParser:
|
cdef cppclass CANParser:
|
||||||
|
@ -71,3 +76,7 @@ cdef extern from "common.h":
|
||||||
CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions])
|
CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions])
|
||||||
void update_string(string, bool)
|
void update_string(string, bool)
|
||||||
vector[SignalValue] query_latest()
|
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
|
#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 set_value(uint64_t ret, Signal sig, int64_t ival){
|
||||||
uint64_t ReverseBytes(uint64_t x) {
|
int shift = sig.is_little_endian? sig.b1 : sig.bo;
|
||||||
return ((x & 0xff00000000000000ull) >> 56) |
|
uint64_t mask = ((1ULL << sig.b2)-1) << shift;
|
||||||
((x & 0x00ff000000000000ull) >> 40) |
|
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift;
|
||||||
((x & 0x0000ff0000000000ull) >> 24) |
|
if (sig.is_little_endian) {
|
||||||
((x & 0x000000ff00000000ull) >> 8) |
|
dat = ReverseBytes(dat);
|
||||||
((x & 0x00000000ff000000ull) << 8) |
|
mask = ReverseBytes(mask);
|
||||||
((x & 0x0000000000ff0000ull) << 24) |
|
|
||||||
((x & 0x000000000000ff00ull) << 40) |
|
|
||||||
((x & 0x00000000000000ffull) << 56);
|
|
||||||
}
|
}
|
||||||
|
ret &= ~mask;
|
||||||
|
ret |= dat;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){
|
CANPacker::CANPacker(const std::string& dbc_name) {
|
||||||
int shift = sig.is_little_endian? sig.b1 : sig.bo;
|
dbc = dbc_lookup(dbc_name);
|
||||||
uint64_t mask = ((1ULL << sig.b2)-1) << shift;
|
assert(dbc);
|
||||||
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift;
|
|
||||||
if (sig.is_little_endian) {
|
for (int i=0; i<dbc->num_msgs; i++) {
|
||||||
dat = ReverseBytes(dat);
|
const Msg* msg = &dbc->msgs[i];
|
||||||
mask = ReverseBytes(mask);
|
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 {
|
uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
|
||||||
public:
|
uint64_t ret = 0;
|
||||||
CANPacker(const std::string& dbc_name) {
|
for (const auto& sigval : signals) {
|
||||||
dbc = dbc_lookup(dbc_name);
|
std::string name = std::string(sigval.name);
|
||||||
assert(dbc);
|
double value = sigval.value;
|
||||||
|
|
||||||
for (int i=0; i<dbc->num_msgs; i++) {
|
auto sig_it = signal_lookup.find(std::make_pair(address, name));
|
||||||
const Msg* msg = &dbc->msgs[i];
|
if (sig_it == signal_lookup.end()) {
|
||||||
message_lookup[msg->address] = *msg;
|
WARN("undefined signal %s - %d\n", name.c_str(), address);
|
||||||
for (int j=0; j<msg->num_sigs; j++) {
|
continue;
|
||||||
const Signal* sig = &msg->sigs[j];
|
}
|
||||||
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
|
auto sig = sig_it->second;
|
||||||
}
|
|
||||||
}
|
int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor));
|
||||||
init_crc_lookup_tables();
|
if (ival < 0) {
|
||||||
|
ival = (1ULL << sig.b2) + ival;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
|
ret = set_value(ret, sig, ival);
|
||||||
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");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
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;
|
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:
|
ret = set_value(ret, sig, counter);
|
||||||
const DBC *dbc = NULL;
|
}
|
||||||
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
|
|
||||||
std::map<uint32_t, Msg> message_lookup;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
|
||||||
|
if (sig_it_checksum != signal_lookup.end()) {
|
||||||
extern "C" {
|
auto sig = sig_it_checksum->second;
|
||||||
void* canpack_init(const char* dbc_name) {
|
if (sig.type == SignalType::HONDA_CHECKSUM) {
|
||||||
CANPacker *ret = new CANPacker(std::string(dbc_name));
|
unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size);
|
||||||
return (void*)ret;
|
ret = set_value(ret, sig, chksm);
|
||||||
}
|
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
|
||||||
|
unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size);
|
||||||
uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals, int counter, bool checksum) {
|
ret = set_value(ret, sig, chksm);
|
||||||
CANPacker *cp = (CANPacker*)inst;
|
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
|
||||||
|
// FIXME: Hackish fix for an endianness issue. The message is in reverse byte order
|
||||||
return cp->pack(address, std::vector<SignalPackValue>(vals, vals+num_vals), counter);
|
// 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);
|
||||||
uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
|
ret = set_value(ret, sig, chksm);
|
||||||
CANPacker *cp = (CANPacker*)inst;
|
} else {
|
||||||
return cp->pack(address, signals, counter);
|
//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.string cimport string
|
||||||
from libcpp cimport bool
|
from libcpp cimport bool
|
||||||
from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY
|
from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY
|
||||||
import os
|
|
||||||
import subprocess
|
|
||||||
|
|
||||||
cdef struct SignalPackValue:
|
from common cimport CANPacker as cpp_CANPacker
|
||||||
const char* name
|
from common cimport dbc_lookup, SignalPackValue, DBC
|
||||||
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 class CANPacker:
|
||||||
cdef struct Msg:
|
cdef:
|
||||||
const char* name
|
cpp_CANPacker *packer
|
||||||
uint32_t address
|
const DBC *dbc
|
||||||
unsigned int size
|
map[string, (int, int)] name_to_address_and_size
|
||||||
size_t num_sigs
|
map[int, int] address_to_size
|
||||||
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):
|
def __init__(self, dbc_name):
|
||||||
can_dir = os.path.dirname(os.path.abspath(__file__))
|
self.packer = new cpp_CANPacker(dbc_name)
|
||||||
libdbc_fn = os.path.join(can_dir, "libdbc.so")
|
self.dbc = dbc_lookup(dbc_name)
|
||||||
libdbc_fn = str(libdbc_fn).encode('utf8')
|
|
||||||
|
|
||||||
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
|
num_msgs = self.dbc[0].num_msgs
|
||||||
for i in range(num_msgs):
|
for i in range(num_msgs):
|
||||||
msg = self.dbc[0].msgs[i]
|
msg = self.dbc[0].msgs[i]
|
||||||
|
@ -99,7 +43,7 @@ cdef class CANPacker():
|
||||||
spv.value = value
|
spv.value = value
|
||||||
values_thing.push_back(spv)
|
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):
|
cdef inline uint64_t ReverseBytes(self, uint64_t x):
|
||||||
return (((x & 0xff00000000000000ull) >> 56) |
|
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 distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module
|
||||||
|
|
||||||
from Cython.Build import cythonize
|
from Cython.Build import cythonize
|
||||||
|
|
||||||
|
from common.basedir import BASEDIR
|
||||||
from common.cython_hacks import BuildExtWithoutPlatformSuffix
|
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},
|
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