Skip to content
This repository has been archived by the owner on Apr 13, 2024. It is now read-only.

Commit

Permalink
unify can packer and parser
Browse files Browse the repository at this point in the history
  • Loading branch information
@pd0wm
pd0wm committed Dec 3, 2019
1 parent 25d8800 commit 0ba7926
Show file tree
Hide file tree
Showing 5 changed files with 140 additions and 180 deletions.
11 changes: 11 additions & 0 deletions can/common.h
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once

#include <vector>
#include <map>
#include <unordered_map>

#include "common_dbc.h"
Expand Down Expand Up @@ -55,5 +56,15 @@ class CANParser {
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);
};
13 changes: 11 additions & 2 deletions can/common.pxd
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -64,10 +63,20 @@ 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:
bool can_valid
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)
187 changes: 78 additions & 109 deletions can/packer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

CANPacker::CANPacker(const std::string& dbc_name) {
dbc = dbc_lookup(dbc_name);
assert(dbc);

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);
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;

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);
}
int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor));
if (ival < 0) {
ival = (1ULL << sig.b2) + ival;
}

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;
};

}

extern "C" {
void* canpack_init(const char* dbc_name) {
CANPacker *ret = new CANPacker(std::string(dbc_name));
return (void*)ret;
ret = set_value(ret, sig, counter);
}

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);
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");
}
}

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);
}
return ret;
}
78 changes: 11 additions & 67 deletions can/packer_pyx.pyx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
from common cimport CANPacker as cpp_CANPacker
from common cimport dbc_lookup, SignalPackValue, DBC

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
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')

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 = new cpp_CANPacker(dbc_name)
self.dbc = dbc_lookup(dbc_name)

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]
Expand All @@ -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) |
Expand Down
Loading

0 comments on commit 0ba7926

Please sign in to comment.