Skip to content

Commit

Permalink
refactor parser
Browse files Browse the repository at this point in the history
  • Loading branch information
deanlee committed Aug 9, 2024
1 parent f4d077b commit ff068a1
Show file tree
Hide file tree
Showing 5 changed files with 117 additions and 120 deletions.
10 changes: 5 additions & 5 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 @@ -45,8 +46,7 @@ class MessageState {
unsigned int size;

std::vector<Signal> parse_sigs;
std::vector<double> vals;
std::vector<std::vector<double>> all_vals;
std::map<std::string, SignalValue> values;

uint64_t last_seen_nanos;
uint64_t check_threshold;
Expand Down Expand Up @@ -79,11 +79,11 @@ class CANParser {
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);
MessageState *messageState(uint32_t address) { return &message_states.at(address); }
std::set<uint32_t> update(const std::vector<CanData> &can_data);

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

Expand Down
10 changes: 7 additions & 3 deletions opendbc/can/common.pxd
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,9 @@

from libc.stdint cimport uint8_t, uint32_t, uint64_t
from libcpp cimport bool
from libcpp.map cimport map
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 @@ -53,9 +55,7 @@ cdef extern from "common_dbc.h":
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

Expand All @@ -67,6 +67,9 @@ cdef extern from "common_dbc.h":
cdef extern from "common.h":
cdef const DBC* dbc_lookup(const string) except +

cdef cppclass MessageState:
map[string, SignalValue] values

cdef struct CanFrame:
long src
uint32_t address
Expand All @@ -80,7 +83,8 @@ cdef extern from "common.h":
bool can_valid
bool bus_timeout
CANParser(int, string, vector[pair[uint32_t, int]]) except +
void update(vector[CanData]&, vector[SignalValue]&) except +
MessageState *messageState(uint32_t address)
set[uint32_t] update(vector[CanData]&) except +

cdef cppclass CANPacker:
CANPacker(string)
Expand Down
2 changes: 0 additions & 2 deletions opendbc/can/common_dbc.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,7 @@ struct SignalPackValue {
};

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
};
Expand Down
100 changes: 38 additions & 62 deletions opendbc/can/parser.cc
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,6 @@
#include <stdexcept>
#include <sstream>

#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/mman.h>

#include "opendbc/can/common.h"

int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
Expand All @@ -31,7 +26,6 @@ int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
return ret;
}


bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
std::vector<double> tmp_vals(parse_sigs.size());
bool checksum_failed = false;
Expand Down Expand Up @@ -69,8 +63,10 @@ bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
}

for (int i = 0; i < parse_sigs.size(); i++) {
vals[i] = tmp_vals[i];
all_vals[i].push_back(vals[i]);
auto &val = values[parse_sigs[i].name];
val.value = tmp_vals[i];
val.ts_nanos = nanos;
val.all_values.push_back(val.value);
}
last_seen_nanos = nanos;

Expand Down Expand Up @@ -126,8 +122,9 @@ CANParser::CANParser(int abus, const std::string& dbc_name, const std::vector<st

// track all signals for this message
state.parse_sigs = msg->sigs;
state.vals.resize(msg->sigs.size());
state.all_vals.resize(msg->sigs.size());
for (auto &sig : msg->sigs) {
state.values[sig.name] = {};
}
}
}

Expand All @@ -147,44 +144,49 @@ CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum
.ignore_counter = ignore_counter,
};

for (const auto& sig : msg.sigs) {
state.parse_sigs.push_back(sig);
state.vals.push_back(0);
state.all_vals.push_back({});
}

state.parse_sigs = msg.sigs;
message_states[state.address] = state;
for (auto &sig : msg.sigs) {
message_states[state.address].values[sig.name] = {};
}
}
}

void CANParser::update(const std::vector<CanData> &can_data, std::vector<SignalValue> &vals) {
uint64_t current_nanos = 0;
for (const auto &c : can_data) {
if (first_nanos == 0) {
first_nanos = c.nanos;
}
if (current_nanos == 0) {
current_nanos = c.nanos;
std::set<uint32_t> CANParser::update(const std::vector<CanData> &can_data) {
// Clear all_values
for (auto &state : message_states) {
for (auto &value : state.second.values) {
value.second.all_values.clear();
}
last_nanos = c.nanos;
}

std::set<uint32_t> updated_addresses;
if (can_data.empty()) {
return updated_addresses;
}

UpdateCans(c);
UpdateValid(last_nanos);
if (first_nanos == 0) {
first_nanos = can_data.front().nanos;
}

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

return updated_addresses;
}

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;

for (const auto &frame : can.frames) {
if (frame.src != bus) {
// DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
continue;
}
bus_empty = false;
last_nonempty_nanos = can.nanos;

auto state_it = message_states.find(frame.address);
if (state_it == message_states.end()) {
Expand All @@ -202,14 +204,10 @@ void CANParser::UpdateCans(const CanData &can) {
// continue;
//}

state_it->second.parse(can.nanos, frame.dat);
}

// update bus timeout
if (!bus_empty) {
last_nonempty_nanos = can.nanos;
if (state_it->second.parse(can.nanos, frame.dat)) {
updated_addresses.insert(frame.address);
}
}
bus_timeout = (can.nanos - last_nonempty_nanos) > bus_timeout_threshold;
}

void CANParser::UpdateValid(uint64_t nanos) {
Expand Down Expand Up @@ -239,27 +237,5 @@ 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();
}
}
bus_timeout = (nanos - last_nonempty_nanos) > bus_timeout_threshold;
}
Loading

0 comments on commit ff068a1

Please sign in to comment.