Skip to content

Commit 0ba7926

Browse files
pd0wmpd0wm
committed
unify can packer and parser
1 parent 25d8800 commit 0ba7926

File tree

5 files changed

+140
-180
lines changed

5 files changed

+140
-180
lines changed

‎can/common.h

+11
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#pragma once
22

33
#include <vector>
4+
#include <map>
45
#include <unordered_map>
56

67
#include "common_dbc.h"
@@ -55,5 +56,15 @@ class CANParser {
5556
void UpdateValid(uint64_t sec);
5657
void update_string(std::string data, bool sendcan);
5758
std::vector<SignalValue> query_latest();
59+
};
5860

61+
class CANPacker {
62+
private:
63+
const DBC *dbc = NULL;
64+
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
65+
std::map<uint32_t, Msg> message_lookup;
66+
67+
public:
68+
CANPacker(const std::string& dbc_name);
69+
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter);
5970
};

‎can/common.pxd

+11-2
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,7 @@ from libcpp.unordered_set cimport unordered_set
99
from libcpp cimport bool
1010

1111

12-
cdef extern from "common.h":
13-
12+
cdef extern from "common_dbc.h":
1413
ctypedef enum SignalType:
1514
DEFAULT,
1615
HONDA_CHECKSUM,
@@ -64,10 +63,20 @@ cdef extern from "common.h":
6463
const char* name
6564
double value
6665

66+
cdef struct SignalPackValue:
67+
const char * name
68+
double value
69+
70+
71+
cdef extern from "common.h":
6772
cdef const DBC* dbc_lookup(const string);
6873

6974
cdef cppclass CANParser:
7075
bool can_valid
7176
CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions])
7277
void update_string(string, bool)
7378
vector[SignalValue] query_latest()
79+
80+
cdef cppclass CANPacker:
81+
CANPacker(string)
82+
uint64_t pack(uint32_t, vector[SignalPackValue], int counter)

‎can/packer.cc

+78-109
Original file line numberDiff line numberDiff line change
@@ -8,132 +8,101 @@
88

99
#define WARN printf
1010

11-
namespace {
11+
// this is the same as read_u64_le, but uses uint64_t as in/out
12+
uint64_t ReverseBytes(uint64_t x) {
13+
return ((x & 0xff00000000000000ull) >> 56) |
14+
((x & 0x00ff000000000000ull) >> 40) |
15+
((x & 0x0000ff0000000000ull) >> 24) |
16+
((x & 0x000000ff00000000ull) >> 8) |
17+
((x & 0x00000000ff000000ull) << 8) |
18+
((x & 0x0000000000ff0000ull) << 24) |
19+
((x & 0x000000000000ff00ull) << 40) |
20+
((x & 0x00000000000000ffull) << 56);
21+
}
1222

13-
// this is the same as read_u64_le, but uses uint64_t as in/out
14-
uint64_t ReverseBytes(uint64_t x) {
15-
return ((x & 0xff00000000000000ull) >> 56) |
16-
((x & 0x00ff000000000000ull) >> 40) |
17-
((x & 0x0000ff0000000000ull) >> 24) |
18-
((x & 0x000000ff00000000ull) >> 8) |
19-
((x & 0x00000000ff000000ull) << 8) |
20-
((x & 0x0000000000ff0000ull) << 24) |
21-
((x & 0x000000000000ff00ull) << 40) |
22-
((x & 0x00000000000000ffull) << 56);
23+
uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){
24+
int shift = sig.is_little_endian? sig.b1 : sig.bo;
25+
uint64_t mask = ((1ULL << sig.b2)-1) << shift;
26+
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift;
27+
if (sig.is_little_endian) {
28+
dat = ReverseBytes(dat);
29+
mask = ReverseBytes(mask);
2330
}
31+
ret &= ~mask;
32+
ret |= dat;
33+
return ret;
34+
}
35+
36+
CANPacker::CANPacker(const std::string& dbc_name) {
37+
dbc = dbc_lookup(dbc_name);
38+
assert(dbc);
2439

25-
uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){
26-
int shift = sig.is_little_endian? sig.b1 : sig.bo;
27-
uint64_t mask = ((1ULL << sig.b2)-1) << shift;
28-
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift;
29-
if (sig.is_little_endian) {
30-
dat = ReverseBytes(dat);
31-
mask = ReverseBytes(mask);
40+
for (int i=0; i<dbc->num_msgs; i++) {
41+
const Msg* msg = &dbc->msgs[i];
42+
message_lookup[msg->address] = *msg;
43+
for (int j=0; j<msg->num_sigs; j++) {
44+
const Signal* sig = &msg->sigs[j];
45+
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
3246
}
33-
ret &= ~mask;
34-
ret |= dat;
35-
return ret;
3647
}
48+
init_crc_lookup_tables();
49+
}
3750

38-
class CANPacker {
39-
public:
40-
CANPacker(const std::string& dbc_name) {
41-
dbc = dbc_lookup(dbc_name);
42-
assert(dbc);
51+
uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
52+
uint64_t ret = 0;
53+
for (const auto& sigval : signals) {
54+
std::string name = std::string(sigval.name);
55+
double value = sigval.value;
4356

44-
for (int i=0; i<dbc->num_msgs; i++) {
45-
const Msg* msg = &dbc->msgs[i];
46-
message_lookup[msg->address] = *msg;
47-
for (int j=0; j<msg->num_sigs; j++) {
48-
const Signal* sig = &msg->sigs[j];
49-
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
50-
}
51-
}
52-
init_crc_lookup_tables();
57+
auto sig_it = signal_lookup.find(std::make_pair(address, name));
58+
if (sig_it == signal_lookup.end()) {
59+
WARN("undefined signal %s - %d\n", name.c_str(), address);
60+
continue;
5361
}
62+
auto sig = sig_it->second;
5463

55-
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
56-
uint64_t ret = 0;
57-
for (const auto& sigval : signals) {
58-
std::string name = std::string(sigval.name);
59-
double value = sigval.value;
60-
61-
auto sig_it = signal_lookup.find(std::make_pair(address, name));
62-
if (sig_it == signal_lookup.end()) {
63-
WARN("undefined signal %s - %d\n", name.c_str(), address);
64-
continue;
65-
}
66-
auto sig = sig_it->second;
67-
68-
int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor));
69-
if (ival < 0) {
70-
ival = (1ULL << sig.b2) + ival;
71-
}
72-
73-
ret = set_value(ret, sig, ival);
74-
}
75-
76-
if (counter >= 0){
77-
auto sig_it = signal_lookup.find(std::make_pair(address, "COUNTER"));
78-
if (sig_it == signal_lookup.end()) {
79-
WARN("COUNTER not defined\n");
80-
return ret;
81-
}
82-
auto sig = sig_it->second;
83-
84-
if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) {
85-
WARN("COUNTER signal type not valid\n");
86-
}
87-
88-
ret = set_value(ret, sig, counter);
89-
}
64+
int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor));
65+
if (ival < 0) {
66+
ival = (1ULL << sig.b2) + ival;
67+
}
9068

91-
auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
92-
if (sig_it_checksum != signal_lookup.end()) {
93-
auto sig = sig_it_checksum->second;
94-
if (sig.type == SignalType::HONDA_CHECKSUM) {
95-
unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size);
96-
ret = set_value(ret, sig, chksm);
97-
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
98-
unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size);
99-
ret = set_value(ret, sig, chksm);
100-
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
101-
// FIXME: Hackish fix for an endianness issue. The message is in reverse byte order
102-
// until later in the pack process. Checksums can be run backwards, CRCs not so much.
103-
// The correct fix is unclear but this works for the moment.
104-
unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size);
105-
ret = set_value(ret, sig, chksm);
106-
} else {
107-
//WARN("CHECKSUM signal type not valid\n");
108-
}
109-
}
69+
ret = set_value(ret, sig, ival);
70+
}
11071

72+
if (counter >= 0){
73+
auto sig_it = signal_lookup.find(std::make_pair(address, "COUNTER"));
74+
if (sig_it == signal_lookup.end()) {
75+
WARN("COUNTER not defined\n");
11176
return ret;
11277
}
78+
auto sig = sig_it->second;
11379

80+
if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) {
81+
WARN("COUNTER signal type not valid\n");
82+
}
11483

115-
private:
116-
const DBC *dbc = NULL;
117-
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
118-
std::map<uint32_t, Msg> message_lookup;
119-
};
120-
121-
}
122-
123-
extern "C" {
124-
void* canpack_init(const char* dbc_name) {
125-
CANPacker *ret = new CANPacker(std::string(dbc_name));
126-
return (void*)ret;
84+
ret = set_value(ret, sig, counter);
12785
}
12886

129-
uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals, int counter, bool checksum) {
130-
CANPacker *cp = (CANPacker*)inst;
131-
132-
return cp->pack(address, std::vector<SignalPackValue>(vals, vals+num_vals), counter);
87+
auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
88+
if (sig_it_checksum != signal_lookup.end()) {
89+
auto sig = sig_it_checksum->second;
90+
if (sig.type == SignalType::HONDA_CHECKSUM) {
91+
unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size);
92+
ret = set_value(ret, sig, chksm);
93+
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
94+
unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size);
95+
ret = set_value(ret, sig, chksm);
96+
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
97+
// FIXME: Hackish fix for an endianness issue. The message is in reverse byte order
98+
// until later in the pack process. Checksums can be run backwards, CRCs not so much.
99+
// The correct fix is unclear but this works for the moment.
100+
unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size);
101+
ret = set_value(ret, sig, chksm);
102+
} else {
103+
//WARN("CHECKSUM signal type not valid\n");
104+
}
133105
}
134106

135-
uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
136-
CANPacker *cp = (CANPacker*)inst;
137-
return cp->pack(address, signals, counter);
138-
}
107+
return ret;
139108
}

‎can/packer_pyx.pyx

+11-67
Original file line numberDiff line numberDiff line change
@@ -7,78 +7,22 @@ from libcpp.map cimport map
77
from libcpp.string cimport string
88
from libcpp cimport bool
99
from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY
10-
import os
11-
import subprocess
1210

13-
cdef struct SignalPackValue:
14-
const char* name
15-
double value
11+
from common cimport CANPacker as cpp_CANPacker
12+
from common cimport dbc_lookup, SignalPackValue, DBC
1613

17-
ctypedef enum SignalType:
18-
DEFAULT,
19-
HONDA_CHECKSUM,
20-
HONDA_COUNTER,
21-
TOYOTA_CHECKSUM,
22-
PEDAL_CHECKSUM,
23-
PEDAL_COUNTER,
24-
VOLKSWAGEN_CHECKSUM,
25-
VOLKSWAGEN_COUNTER
2614

27-
cdef struct Signal:
28-
const char* name
29-
int b1, b2, bo
30-
bool is_signed
31-
double factor, offset
32-
SignalType type
33-
34-
35-
36-
cdef struct Msg:
37-
const char* name
38-
uint32_t address
39-
unsigned int size
40-
size_t num_sigs
41-
const Signal *sigs
42-
43-
cdef struct Val:
44-
const char* name
45-
uint32_t address
46-
const char* def_val
47-
const Signal *sigs
48-
49-
cdef struct DBC:
50-
const char* name
51-
size_t num_msgs
52-
const Msg *msgs
53-
const Val *vals
54-
size_t num_vals
55-
56-
ctypedef void * (*canpack_init_func)(const char* dbc_name)
57-
ctypedef uint64_t (*canpack_pack_vector_func)(void* inst, uint32_t address, const vector[SignalPackValue] &signals, int counter)
58-
ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name)
59-
60-
61-
cdef class CANPacker():
62-
cdef void *packer
63-
cdef const DBC *dbc
64-
cdef map[string, (int, int)] name_to_address_and_size
65-
cdef map[int, int] address_to_size
66-
cdef canpack_init_func canpack_init
67-
cdef canpack_pack_vector_func canpack_pack_vector
68-
cdef dbc_lookup_func dbc_lookup
15+
cdef class CANPacker:
16+
cdef:
17+
cpp_CANPacker *packer
18+
const DBC *dbc
19+
map[string, (int, int)] name_to_address_and_size
20+
map[int, int] address_to_size
6921

7022
def __init__(self, dbc_name):
71-
can_dir = os.path.dirname(os.path.abspath(__file__))
72-
libdbc_fn = os.path.join(can_dir, "libdbc.so")
73-
libdbc_fn = str(libdbc_fn).encode('utf8')
74-
75-
cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY)
76-
self.canpack_init = <canpack_init_func>dlsym(libdbc, 'canpack_init')
77-
self.canpack_pack_vector = <canpack_pack_vector_func>dlsym(libdbc, 'canpack_pack_vector')
78-
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
23+
self.packer = new cpp_CANPacker(dbc_name)
24+
self.dbc = dbc_lookup(dbc_name)
7925

80-
self.packer = self.canpack_init(dbc_name)
81-
self.dbc = self.dbc_lookup(dbc_name)
8226
num_msgs = self.dbc[0].num_msgs
8327
for i in range(num_msgs):
8428
msg = self.dbc[0].msgs[i]
@@ -99,7 +43,7 @@ cdef class CANPacker():
9943
spv.value = value
10044
values_thing.push_back(spv)
10145

102-
return self.canpack_pack_vector(self.packer, addr, values_thing, counter)
46+
return self.packer.pack(addr, values_thing, counter)
10347

10448
cdef inline uint64_t ReverseBytes(self, uint64_t x):
10549
return (((x & 0xff00000000000000ull) >> 56) |

0 commit comments

Comments
 (0)