Skip to content

Commit

Permalink
refactor update
Browse files Browse the repository at this point in the history
update test case

improve update()
  • Loading branch information
deanlee committed Aug 3, 2024
1 parent 5c16f7e commit f9f194f
Show file tree
Hide file tree
Showing 6 changed files with 84 additions and 103 deletions.
9 changes: 5 additions & 4 deletions opendbc/can/common.h
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once

#include <map>
#include <set>
#include <string>
#include <utility>
#include <unordered_map>
Expand Down Expand Up @@ -65,7 +66,6 @@ class CANParser {
private:
const int bus;
const DBC *dbc = NULL;
std::unordered_map<uint32_t, MessageState> message_states;

public:
bool can_valid = false;
Expand All @@ -75,15 +75,16 @@ class CANParser {
uint64_t last_nonempty_nanos = 0;
uint64_t bus_timeout_threshold = 0;
uint64_t can_invalid_cnt = CAN_INVALID_CNT;
std::unordered_map<uint32_t, MessageState> message_states;

CANParser(int abus, const std::string& dbc_name,
const std::vector<std::pair<uint32_t, int>> &messages);
CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter);
void update(const std::vector<CanData> &can_data, std::vector<SignalValue> &vals);
void query_latest(std::vector<SignalValue> &vals, uint64_t last_ts = 0);
std::set<uint32_t> update(const std::vector<CanData> &can_data);

protected:
void UpdateCans(const CanData &can);
void clearAllValues();
void updateCans(const CanData &can, std::set<uint32_t> &updated_addresses);
void UpdateValid(uint64_t nanos);
};

Expand Down
17 changes: 9 additions & 8 deletions opendbc/can/common.pxd
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
from libc.stdint cimport uint8_t, uint32_t, uint64_t
from libcpp cimport bool
from libcpp.pair cimport pair
from libcpp.set cimport set
from libcpp.string cimport string
from libcpp.vector cimport vector
from libcpp.unordered_map cimport unordered_map
Expand Down Expand Up @@ -52,13 +53,6 @@ cdef extern from "common_dbc.h":
unordered_map[uint32_t, const Msg*] addr_to_msg
unordered_map[string, const Msg*] name_to_msg

cdef struct SignalValue:
uint32_t address
uint64_t ts_nanos
string name
double value
vector[double] all_values

cdef struct SignalPackValue:
string name
double value
Expand All @@ -76,11 +70,18 @@ cdef extern from "common.h":
uint64_t nanos
vector[CanFrame] frames

cdef cppclass MessageState:
vector[Signal] parse_sigs
vector[double] vals
vector[vector[double]] all_vals
uint64_t last_seen_nanos

cdef cppclass CANParser:
bool can_valid
bool bus_timeout
unordered_map[uint32_t, MessageState] message_states
CANParser(int, string, vector[pair[uint32_t, int]]) except +
void update(vector[CanData]&, vector[SignalValue]&) except +
set[uint32_t] update(vector[CanData]&) except +

cdef cppclass CANPacker:
CANPacker(string)
Expand Down
8 changes: 0 additions & 8 deletions opendbc/can/common_dbc.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,14 +10,6 @@ struct SignalPackValue {
double value;
};

struct SignalValue {
uint32_t address;
uint64_t ts_nanos;
std::string name;
double value; // latest value
std::vector<double> all_values; // all values from this cycle
};

enum SignalType {
DEFAULT,
COUNTER,
Expand Down
63 changes: 27 additions & 36 deletions opendbc/can/parser.cc
Original file line number Diff line number Diff line change
Expand Up @@ -157,24 +157,36 @@ CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum
}
}

void CANParser::update(const std::vector<CanData> &can_data, std::vector<SignalValue> &vals) {
uint64_t current_nanos = 0;
std::set<uint32_t> CANParser::update(const std::vector<CanData> &can_data) {
clearAllValues();
std::set<uint32_t> updated_addresses;

if (can_data.empty()) {
return updated_addresses;
}

if (first_nanos == 0) {
first_nanos = can_data.front().nanos;
}

for (const auto &c : can_data) {
if (first_nanos == 0) {
first_nanos = c.nanos;
}
if (current_nanos == 0) {
current_nanos = c.nanos;
}
last_nanos = c.nanos;
updateCans(c, updated_addresses);
}
UpdateValid(last_nanos);

return updated_addresses;
}

UpdateCans(c);
UpdateValid(last_nanos);
void CANParser::clearAllValues() {
for (auto &[_, state] : message_states) {
for (auto &vals : state.all_vals) {
vals.clear();
}
}
query_latest(vals, current_nanos);
}

void CANParser::UpdateCans(const CanData &can) {
void CANParser::updateCans(const CanData &can, std::set<uint32_t> &updated_addresses) {
//DEBUG("got %zu messages\n", can.frames.size());

bool bus_empty = true;
Expand Down Expand Up @@ -202,7 +214,9 @@ void CANParser::UpdateCans(const CanData &can) {
// continue;
//}

state_it->second.parse(can.nanos, frame.dat);
if (state_it->second.parse(can.nanos, frame.dat)) {
updated_addresses.insert(frame.address);
}
}

// update bus timeout
Expand Down Expand Up @@ -240,26 +254,3 @@ void CANParser::UpdateValid(uint64_t nanos) {
can_invalid_cnt = _valid ? 0 : (can_invalid_cnt + 1);
can_valid = (can_invalid_cnt < CAN_INVALID_CNT) && _counters_valid;
}

void CANParser::query_latest(std::vector<SignalValue> &vals, uint64_t last_ts) {
if (last_ts == 0) {
last_ts = last_nanos;
}
for (auto& kv : message_states) {
auto& state = kv.second;
if (last_ts != 0 && state.last_seen_nanos < last_ts) {
continue;
}

for (int i = 0; i < state.parse_sigs.size(); i++) {
const Signal &sig = state.parse_sigs[i];
SignalValue &v = vals.emplace_back();
v.address = state.address;
v.ts_nanos = state.last_seen_nanos;
v.name = sig.name;
v.value = state.vals[i];
v.all_values = state.all_vals[i];
state.all_vals[i].clear();
}
}
}
83 changes: 37 additions & 46 deletions opendbc/can/parser_pyx.pyx
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
# distutils: language = c++
# cython: c_string_encoding=ascii, language_level=3

from cython.operator cimport dereference as deref, preincrement as preinc
from libcpp.pair cimport pair
from libcpp.set cimport set
from libcpp.string cimport string
from libcpp.vector cimport vector
from libc.stdint cimport uint32_t

from .common cimport CANParser as cpp_CANParser
from .common cimport dbc_lookup, SignalValue, DBC, CanData, CanFrame
from .common cimport dbc_lookup, DBC, CanData, CanFrame

import numbers
from collections import defaultdict
Expand All @@ -18,15 +18,17 @@ cdef class CANParser:
cdef:
cpp_CANParser *can
const DBC *dbc
vector[uint32_t] addresses
set[uint32_t] addresses

cdef readonly:
dict vl
dict vl_all
dict ts_nanos
string dbc_name
int bus

def __init__(self, dbc_name, messages, bus=0):
self.bus = bus
self.dbc_name = dbc_name
self.dbc = dbc_lookup(dbc_name)
if not self.dbc:
Expand All @@ -47,18 +49,11 @@ cdef class CANParser:

address = m.address
message_v.push_back((address, c[1]))
self.addresses.push_back(address)

name = m.name.decode("utf8")
self.vl[address] = {}
self.vl[name] = self.vl[address]
self.vl_all[address] = defaultdict(list)
self.vl_all[name] = self.vl_all[address]
self.ts_nanos[address] = {}
self.ts_nanos[name] = self.ts_nanos[address]
self.addresses.insert(address)

self.can = new cpp_CANParser(bus, dbc_name, message_v)
self.update_strings([])
self._update_value_dicts(self.addresses)
self._map_dicts_by_name_to_address()

def __dealloc__(self):
if self.can:
Expand All @@ -68,16 +63,7 @@ cdef class CANParser:
# input format:
# [nanos, [[address, data, src], ...]]
# [[nanos, [[address, data, src], ...], ...]]
for address in self.addresses:
self.vl_all[address].clear()

cur_address = -1
vl = {}
vl_all = {}
ts_nanos = {}
updated_addrs = set()

cdef vector[SignalValue] new_vals
cdef CanFrame* frame
cdef CanData* can_data
cdef vector[CanData] can_data_array
Expand All @@ -91,37 +77,42 @@ cdef class CANParser:
can_data = &(can_data_array.emplace_back())
can_data.nanos = s[0]
can_data.frames.reserve(len(s[1]))
for f in s[1]:
for f in (f for f in s[1] if f[2] == self.bus):
frame = &(can_data.frames.emplace_back())
frame.address = f[0]
frame.dat = f[1]
frame.src = f[2]
except TypeError:
raise RuntimeError("invalid parameter")

self.can.update(can_data_array, new_vals)

cdef vector[SignalValue].iterator it = new_vals.begin()
cdef SignalValue* cv
while it != new_vals.end():
cv = &deref(it)

# Check if the address has changed
if cv.address != cur_address:
cur_address = cv.address
vl = self.vl[cur_address]
vl_all = self.vl_all[cur_address]
ts_nanos = self.ts_nanos[cur_address]
updated_addrs.add(cur_address)

# Cast char * directly to unicode
cv_name = <unicode>cv.name
vl[cv_name] = cv.value
vl_all[cv_name] = cv.all_values
ts_nanos[cv_name] = cv.ts_nanos
preinc(it)

return updated_addrs
updated_addresses = self.can.update(can_data_array)
self._update_value_dicts(updated_addresses)
return updated_addresses

cdef _update_value_dicts(self, set[uint32_t] &addresses):
for addr in self.addresses:
if addr in self.vl_all:
self.vl_all[addr].clear()

for addr in addresses:
vl = self.vl.setdefault(addr, {})
vl_all = self.vl_all.setdefault(addr, defaultdict(list))
ts_nanos = self.ts_nanos.setdefault(addr, {})

state = &self.can.message_states.at(addr)
for i in range(state.parse_sigs.size()):
sig_name = <unicode>state.parse_sigs[i].name
vl[sig_name] = state.vals[i]
vl_all[sig_name] = state.all_vals[i]
ts_nanos[sig_name] = state.last_seen_nanos

cdef _map_dicts_by_name_to_address(self):
for address in self.addresses:
msg = self.dbc.addr_to_msg.at(address)
name = <unicode>msg.name
self.vl[name] = self.vl[address]
self.vl_all[name] = self.vl_all[address]
self.ts_nanos[name] = self.ts_nanos[address]

@property
def can_valid(self):
Expand Down
7 changes: 6 additions & 1 deletion opendbc/can/tests/test_packer_parser.py
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,11 @@ def rx_steering_msg(values, bad_checksum=False):
assert parser.vl["STEERING_CONTROL"]["STEER_TORQUE"] == 300
assert parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"] == [300]

def test_parser_empty_message(self):
parser = CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 0)])
addr = parser.update_strings([])
assert len(addr) == 0

def test_packer_parser(self):
msgs = [
("Brake_Status", 0),
Expand Down Expand Up @@ -264,7 +269,7 @@ def test_updated(self):
can_msgs[frame].append(packer.make_can_msg("VSA_STATUS", 0, values))
idx += 1

parser.update_strings([[0, m] for m in can_msgs])
parser.update_strings([[random.randint(0, 255), m] for m in can_msgs])
vl_all = parser.vl_all["VSA_STATUS"]["USER_BRAKE"]

assert vl_all == user_brake_vals
Expand Down

0 comments on commit f9f194f

Please sign in to comment.