mirror of
https://github.com/infiniteCable2/opendbc.git
synced 2026-02-18 13:03:52 +08:00
CANParser: python rewrite (#2539)
* Add pure Python CAN parser * cleanup * lil more * rm cython * cleanup
This commit is contained in:
Adeeb Shihadeh
@@ -1,6 +1,5 @@
|
||||
Import("env")
|
||||
|
||||
SConscript(['opendbc/can/SConscript'], exports={'env': env})
|
||||
SConscript(['opendbc/dbc/SConscript'], exports={'env': env})
|
||||
|
||||
# test files
|
||||
|
||||
32
SConstruct
32
SConstruct
@@ -1,18 +1,14 @@
|
||||
import os
|
||||
import subprocess
|
||||
import sysconfig
|
||||
import platform
|
||||
import numpy as np
|
||||
|
||||
arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
|
||||
if platform.system() == "Darwin":
|
||||
arch = "Darwin"
|
||||
|
||||
python_path = sysconfig.get_paths()['include']
|
||||
cpppath = [
|
||||
'#',
|
||||
'/usr/lib/include',
|
||||
python_path
|
||||
]
|
||||
|
||||
AddOption('--minimal',
|
||||
@@ -53,40 +49,16 @@ env = Environment(
|
||||
] + ccflags_asan,
|
||||
LDFLAGS=ldflags_asan,
|
||||
LINKFLAGS=ldflags_asan,
|
||||
LIBPATH=[
|
||||
"#opendbc/can/",
|
||||
],
|
||||
CFLAGS="-std=gnu11",
|
||||
CXXFLAGS=["-std=c++1z"],
|
||||
CPPPATH=cpppath,
|
||||
CYTHONCFILESUFFIX=".cpp",
|
||||
tools=["default", "cython", "compilation_db"]
|
||||
tools=["default", "compilation_db"]
|
||||
)
|
||||
if arch != "Darwin":
|
||||
env.Append(CCFLAGS=["-fmax-errors=1", ])
|
||||
|
||||
env.CompilationDatabase('compile_commands.json')
|
||||
|
||||
common = ''
|
||||
Export('env', 'arch', 'common')
|
||||
|
||||
envCython = env.Clone()
|
||||
envCython["CPPPATH"] += [np.get_include()]
|
||||
envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"]
|
||||
envCython["CCFLAGS"].remove("-Werror")
|
||||
|
||||
python_libs = []
|
||||
if arch == "Darwin":
|
||||
envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"]
|
||||
elif arch == "aarch64":
|
||||
envCython["LINKFLAGS"] = ["-shared"]
|
||||
|
||||
python_libs.append(os.path.basename(python_path))
|
||||
else:
|
||||
envCython["LINKFLAGS"] = ["-pthread", "-shared"]
|
||||
|
||||
envCython["LIBS"] = python_libs
|
||||
|
||||
Export('envCython')
|
||||
Export('env', 'arch')
|
||||
|
||||
SConscript(['SConscript'])
|
||||
|
||||
@@ -15,8 +15,6 @@ test:
|
||||
# *** static analysis
|
||||
ruff:
|
||||
run: ruff check .
|
||||
cython-lint:
|
||||
run: cython-lint opendbc/
|
||||
codespell:
|
||||
run: codespell {files} -L tge,stdio -S *.dbc
|
||||
files: git ls-tree -r HEAD --name-only
|
||||
|
||||
@@ -1,22 +0,0 @@
|
||||
Import('env', 'envCython', 'common', 'arch')
|
||||
|
||||
envDBC = env.Clone()
|
||||
dbc_file_path = '-DDBC_FILE_PATH=\'"%s"\'' % (envDBC.Dir("../dbc").abspath)
|
||||
envDBC['CXXFLAGS'] += [dbc_file_path]
|
||||
src = ["dbc.cc", "parser.cc", "car_specific.cc"]
|
||||
|
||||
# shared library for openpilot
|
||||
LINKFLAGS = envDBC["LINKFLAGS"]
|
||||
if arch == "Darwin":
|
||||
LINKFLAGS += ["-Wl,-install_name,@loader_path/libdbc.dylib"]
|
||||
libdbc = envDBC.SharedLibrary('libdbc', src, LIBS=[common, ], LINKFLAGS=LINKFLAGS)
|
||||
|
||||
# Build parser
|
||||
lenv = envCython.Clone()
|
||||
lenv["LIBPATH"].append(Dir("."))
|
||||
lenv["RPATH"] = [lenv.Literal('\\$$ORIGIN')]
|
||||
parser = lenv.Program('parser_pyx.so', 'parser_pyx.pyx', LIBS=[common, libdbc[0].name])
|
||||
|
||||
opendbc_python = Alias("opendbc_python", [parser, ])
|
||||
|
||||
Export('opendbc_python')
|
||||
@@ -1,5 +1,8 @@
|
||||
from opendbc.can.parser_pyx import CANParser, CANDefine
|
||||
assert CANParser, CANDefine
|
||||
|
||||
from opendbc.can.packer import CANPacker
|
||||
assert CANPacker
|
||||
from opendbc.can.parser import CANParser, CANDefine
|
||||
|
||||
__all__ = [
|
||||
"CANDefine",
|
||||
"CANParser",
|
||||
"CANPacker",
|
||||
]
|
||||
|
||||
@@ -1,288 +0,0 @@
|
||||
#include <array>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "opendbc/can/common.h"
|
||||
|
||||
/*
|
||||
TODO: these should move to their respective folders in opendbc/car/
|
||||
*/
|
||||
|
||||
void tesla_setup_signal(Signal &sig, const std::string& dbc_name, int line_num) {
|
||||
if (endswith(sig.name, "Counter")) {
|
||||
sig.type = COUNTER;
|
||||
} else if (endswith(sig.name, "Checksum")) {
|
||||
sig.type = TESLA_CHECKSUM;
|
||||
sig.calc_checksum = &tesla_checksum;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int honda_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
int s = 0;
|
||||
bool extended = address > 0x7FF;
|
||||
while (address) { s += (address & 0xF); address >>= 4; }
|
||||
for (int i = 0; i < d.size(); i++) {
|
||||
uint8_t x = d[i];
|
||||
if (i == d.size()-1) x >>= 4; // remove checksum
|
||||
s += (x & 0xF) + (x >> 4);
|
||||
}
|
||||
s = 8-s;
|
||||
if (extended) s += 3; // extended can
|
||||
|
||||
return s & 0xF;
|
||||
}
|
||||
|
||||
unsigned int toyota_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
unsigned int s = d.size();
|
||||
while (address) { s += address & 0xFF; address >>= 8; }
|
||||
for (int i = 0; i < d.size() - 1; i++) { s += d[i]; }
|
||||
|
||||
return s & 0xFF;
|
||||
}
|
||||
|
||||
unsigned int subaru_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
unsigned int s = 0;
|
||||
while (address) { s += address & 0xFF; address >>= 8; }
|
||||
|
||||
// skip checksum in first byte
|
||||
for (int i = 1; i < d.size(); i++) { s += d[i]; }
|
||||
|
||||
return s & 0xFF;
|
||||
}
|
||||
|
||||
unsigned int chrysler_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
// jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf
|
||||
uint8_t checksum = 0xFF;
|
||||
for (int j = 0; j < (d.size() - 1); j++) {
|
||||
uint8_t shift = 0x80;
|
||||
uint8_t curr = d[j];
|
||||
for (int i = 0; i < 8; i++) {
|
||||
uint8_t bit_sum = curr & shift;
|
||||
uint8_t temp_chk = checksum & 0x80U;
|
||||
if (bit_sum != 0U) {
|
||||
bit_sum = 0x1C;
|
||||
if (temp_chk != 0U) {
|
||||
bit_sum = 1;
|
||||
}
|
||||
checksum = checksum << 1;
|
||||
temp_chk = checksum | 1U;
|
||||
bit_sum ^= temp_chk;
|
||||
} else {
|
||||
if (temp_chk != 0U) {
|
||||
bit_sum = 0x1D;
|
||||
}
|
||||
checksum = checksum << 1;
|
||||
bit_sum ^= checksum;
|
||||
}
|
||||
checksum = bit_sum;
|
||||
shift = shift >> 1;
|
||||
}
|
||||
}
|
||||
return ~checksum & 0xFF;
|
||||
}
|
||||
|
||||
// Static lookup table for fast computation of CRCs
|
||||
uint8_t crc8_lut_8h2f[256]; // CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
||||
uint8_t crc8_lut_j1850[256]; // CRC8 poly 0x1D, aka SAE J1850
|
||||
uint16_t crc16_lut_xmodem[256]; // CRC16 poly 0x1021, aka XMODEM
|
||||
|
||||
void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]) {
|
||||
uint8_t crc;
|
||||
int i, j;
|
||||
|
||||
for (i = 0; i < 256; i++) {
|
||||
crc = i;
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80) != 0)
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
else
|
||||
crc <<= 1;
|
||||
}
|
||||
crc_lut[i] = crc;
|
||||
}
|
||||
}
|
||||
|
||||
void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]) {
|
||||
uint16_t crc;
|
||||
int i, j;
|
||||
|
||||
for (i = 0; i < 256; i++) {
|
||||
crc = i << 8;
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x8000) != 0) {
|
||||
crc = (uint16_t)((crc << 1) ^ poly);
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
crc_lut[i] = crc;
|
||||
}
|
||||
}
|
||||
|
||||
// Initializes CRC lookup tables at module initialization
|
||||
struct CrcInitializer {
|
||||
CrcInitializer() {
|
||||
gen_crc_lookup_table_8(0x2F, crc8_lut_8h2f); // CRC-8 8H2F/AUTOSAR for Volkswagen
|
||||
gen_crc_lookup_table_8(0x1D, crc8_lut_j1850); // CRC-8 SAE-J1850
|
||||
gen_crc_lookup_table_16(0x1021, crc16_lut_xmodem); // CRC-16 XMODEM for HKG CAN FD
|
||||
}
|
||||
};
|
||||
|
||||
static CrcInitializer crcInitializer;
|
||||
|
||||
static const std::unordered_map<uint32_t, std::array<uint8_t, 16>> volkswagen_mqb_meb_crc_constants {
|
||||
{0x40, {0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40}}, // Airbag_01
|
||||
{0x86, {0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86}}, // LWI_01
|
||||
{0x9F, {0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5}}, // LH_EPS_03
|
||||
{0xAD, {0x3F, 0x69, 0x39, 0xDC, 0x94, 0xF9, 0x14, 0x64, 0xD8, 0x6A, 0x34, 0xCE, 0xA2, 0x55, 0xB5, 0x2C}}, // Getriebe_11
|
||||
{0x0DB, {0x09, 0xFA, 0xCA, 0x8E, 0x62, 0xD5, 0xD1, 0xF0, 0x31, 0xA0, 0xAF, 0xDA, 0x4D, 0x1A, 0x0A, 0x97}}, // AWV_03
|
||||
{0xFC, {0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}}, // ESC_51
|
||||
{0xFD, {0xB4, 0xEF, 0xF8, 0x49, 0x1E, 0xE5, 0xC2, 0xC0, 0x97, 0x19, 0x3C, 0xC9, 0xF1, 0x98, 0xD6, 0x61}}, // ESP_21
|
||||
{0x101, {0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA}}, // ESP_02
|
||||
{0x102, {0xD7, 0x12, 0x85, 0x7E, 0x0B, 0x34, 0xFA, 0x16, 0x7A, 0x25, 0x2D, 0x8F, 0x04, 0x8E, 0x5D, 0x35}}, // ESC_50
|
||||
{0x106, {0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07}}, // ESP_05
|
||||
{0x10B, {0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}}, // Motor_51
|
||||
{0x116, {0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC}}, // ESP_10
|
||||
{0x117, {0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16, 0x16}}, // ACC_10
|
||||
{0x120, {0xC4, 0xE2, 0x4F, 0xE4, 0xF8, 0x2F, 0x56, 0x81, 0x9F, 0xE5, 0x83, 0x44, 0x05, 0x3F, 0x97, 0xDF}}, // TSK_06
|
||||
{0x121, {0xE9, 0x65, 0xAE, 0x6B, 0x7B, 0x35, 0xE5, 0x5F, 0x4E, 0xC7, 0x86, 0xA2, 0xBB, 0xDD, 0xEB, 0xB4}}, // Motor_20
|
||||
{0x122, {0x37, 0x7D, 0xF3, 0xA9, 0x18, 0x46, 0x6D, 0x4D, 0x3D, 0x71, 0x92, 0x9C, 0xE5, 0x32, 0x10, 0xB9}}, // ACC_06
|
||||
{0x126, {0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA}}, // HCA_01
|
||||
{0x12B, {0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B}}, // GRA_ACC_01
|
||||
{0x12E, {0xF8, 0xE5, 0x97, 0xC9, 0xD6, 0x07, 0x47, 0x21, 0x66, 0xDD, 0xCF, 0x6F, 0xA1, 0x94, 0x74, 0x63}}, // ACC_07
|
||||
{0x139, {0xED, 0x03, 0x1C, 0x13, 0xC6, 0x23, 0x78, 0x7A, 0x8B, 0x40, 0x14, 0x51, 0xBF, 0x68, 0x32, 0xBA}}, // VMM_02
|
||||
{0x13D, {0x20, 0xCA, 0x68, 0xD5, 0x1B, 0x31, 0xE2, 0xDA, 0x08, 0x0A, 0xD4, 0xDE, 0x9C, 0xE4, 0x35, 0x5B}}, // QFK_01
|
||||
{0x14C, {0x16, 0x35, 0x59, 0x15, 0x9A, 0x2A, 0x97, 0xB8, 0x0E, 0x4E, 0x30, 0xCC, 0xB3, 0x07, 0x01, 0xAD}}, // Motor_54
|
||||
{0x14D, {0x1A, 0x65, 0x81, 0x96, 0xC0, 0xDF, 0x11, 0x92, 0xD3, 0x61, 0xC6, 0x95, 0x8C, 0x29, 0x21, 0xB5}}, // ACC_18
|
||||
{0x187, {0x7F, 0xED, 0x17, 0xC2, 0x7C, 0xEB, 0x44, 0x21, 0x01, 0xFA, 0xDB, 0x15, 0x4A, 0x6B, 0x23, 0x05}}, // Motor_EV_01
|
||||
{0x1A4, {0x69, 0xBB, 0x54, 0xE6, 0x4E, 0x46, 0x8D, 0x7B, 0xEA, 0x87, 0xE9, 0xB3, 0x63, 0xCE, 0xF8, 0xBF}}, // EA_01
|
||||
{0x1AB, {0x13, 0x21, 0x9B, 0x6A, 0x9A, 0x62, 0xD4, 0x65, 0x18, 0xF1, 0xAB, 0x16, 0x32, 0x89, 0xE7, 0x26}}, // ESP_33
|
||||
{0x1F0, {0x2F, 0x3C, 0x22, 0x60, 0x18, 0xEB, 0x63, 0x76, 0xC5, 0x91, 0x0F, 0x27, 0x34, 0x04, 0x7F, 0x02}}, // EA_02
|
||||
{0x20A, {0x9D, 0xE8, 0x36, 0xA1, 0xCA, 0x3B, 0x1D, 0x33, 0xE0, 0xD5, 0xBB, 0x5F, 0xAE, 0x3C, 0x31, 0x9F}}, // EML_06
|
||||
{0x26B, {0xCE, 0xCC, 0xBD, 0x69, 0xA1, 0x3C, 0x18, 0x76, 0x0F, 0x04, 0xF2, 0x3A, 0x93, 0x24, 0x19, 0x51}}, // TA_01
|
||||
{0x30C, {0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F}}, // ACC_02
|
||||
{0x30F, {0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C}}, // SWA_01
|
||||
{0x324, {0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27, 0x27}}, // ACC_04
|
||||
{0x3BE, {0x1F, 0x28, 0xC6, 0x85, 0xE6, 0xF8, 0xB0, 0x19, 0x5B, 0x64, 0x35, 0x21, 0xE4, 0xF7, 0x9C, 0x24}}, // Motor_14
|
||||
{0x3C0, {0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3}}, // Klemmen_Status_01
|
||||
{0x3D5, {0xC5, 0x39, 0xC7, 0xF9, 0x92, 0xD8, 0x24, 0xCE, 0xF1, 0xB5, 0x7A, 0xC4, 0xBC, 0x60, 0xE3, 0xD1}}, // Licht_Anf_01
|
||||
{0x65D, {0xAC, 0xB3, 0xAB, 0xEB, 0x7A, 0xE1, 0x3B, 0xF7, 0x73, 0xBA, 0x7C, 0x9E, 0x06, 0x5F, 0x02, 0xD9}}, // ESP_20
|
||||
};
|
||||
|
||||
unsigned int volkswagen_mqb_meb_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
// This is AUTOSAR E2E Profile 2, CRC-8H2F with a "data ID" (varying by message/counter) appended to the payload
|
||||
|
||||
uint8_t crc = 0xFF; // CRC-8H2F initial value
|
||||
|
||||
// CRC over payload first, skipping the first byte where the CRC lives
|
||||
for (int i = 1; i < d.size(); i++) {
|
||||
crc ^= d[i];
|
||||
crc = crc8_lut_8h2f[crc];
|
||||
}
|
||||
|
||||
// Continue CRC over the "data ID"
|
||||
uint8_t counter = d[1] & 0x0F;
|
||||
|
||||
auto crc_const = volkswagen_mqb_meb_crc_constants.find(address);
|
||||
if (crc_const != volkswagen_mqb_meb_crc_constants.end()) {
|
||||
crc ^= crc_const->second[counter];
|
||||
crc = crc8_lut_8h2f[crc];
|
||||
} else {
|
||||
printf("Attempt to CRC check undefined Volkswagen message 0x%02X\n", address);
|
||||
}
|
||||
|
||||
return crc ^ 0xFF; // CRC-8H2F final XOR
|
||||
}
|
||||
|
||||
unsigned int xor_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
uint8_t checksum = 0;
|
||||
int checksum_byte = sig.start_bit / 8;
|
||||
|
||||
// Simple XOR over the payload, except for the byte where the checksum lives.
|
||||
for (int i = 0; i < d.size(); i++) {
|
||||
if (i != checksum_byte) {
|
||||
checksum ^= d[i];
|
||||
}
|
||||
}
|
||||
|
||||
return checksum;
|
||||
}
|
||||
|
||||
unsigned int body_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
uint8_t crc = 0xFF;
|
||||
uint8_t poly = 0xD5; // standard crc8
|
||||
|
||||
// skip checksum byte
|
||||
for (int i = d.size()-2; i >= 0; i--) {
|
||||
crc ^= d[i];
|
||||
for (int j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80) != 0) {
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
unsigned int hkg_can_fd_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
uint16_t crc = 0;
|
||||
|
||||
for (int i = 2; i < d.size(); i++) {
|
||||
crc = (crc << 8) ^ crc16_lut_xmodem[(crc >> 8) ^ d[i]];
|
||||
}
|
||||
|
||||
// Add address to crc
|
||||
crc = (crc << 8) ^ crc16_lut_xmodem[(crc >> 8) ^ ((address >> 0) & 0xFF)];
|
||||
crc = (crc << 8) ^ crc16_lut_xmodem[(crc >> 8) ^ ((address >> 8) & 0xFF)];
|
||||
|
||||
if (d.size() == 8) {
|
||||
crc ^= 0x5f29;
|
||||
} else if (d.size() == 16) {
|
||||
crc ^= 0x041d;
|
||||
} else if (d.size() == 24) {
|
||||
crc ^= 0x819d;
|
||||
} else if (d.size() == 32) {
|
||||
crc ^= 0x9f5b;
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
unsigned int fca_giorgio_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
// CRC is in the last byte, poly is same as SAE J1850 but uses a different init value and final XOR
|
||||
uint8_t crc = 0x00;
|
||||
|
||||
for (int i = 0; i < d.size() - 1; i++) {
|
||||
crc ^= d[i];
|
||||
crc = crc8_lut_j1850[crc];
|
||||
}
|
||||
|
||||
// Final XOR varies for EPS messages, all others use a common value
|
||||
if (address == 0xDE) { // EPS_1
|
||||
return crc ^ 0x10;
|
||||
} else if (address == 0x106) { // EPS_2
|
||||
return crc ^ 0xF6;
|
||||
} else if (address == 0x122) { // EPS_3
|
||||
return crc ^ 0xF1;
|
||||
} else {
|
||||
return crc ^ 0xA;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
unsigned int tesla_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
|
||||
uint8_t checksum = (address & 0xFF) + ((address >> 8) & 0xFF);
|
||||
int checksum_byte = sig.start_bit / 8;
|
||||
|
||||
for (int i = 0; i < d.size(); i++) {
|
||||
if (i != checksum_byte) {
|
||||
checksum += d[i];
|
||||
}
|
||||
}
|
||||
|
||||
return checksum & 0xFF;
|
||||
}
|
||||
@@ -1,110 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <deque>
|
||||
#include <cstring>
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <utility>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "opendbc/can/logger.h"
|
||||
#include "opendbc/can/dbc.h"
|
||||
|
||||
#define INFO printf
|
||||
#define WARN printf
|
||||
#define DEBUG(...)
|
||||
//#define DEBUG printf
|
||||
|
||||
#define MAX_BAD_COUNTER 5
|
||||
#define CAN_INVALID_CNT 5
|
||||
|
||||
// Car specific functions
|
||||
void tesla_setup_signal(Signal &sig, const std::string& dbc_name, int line_num);
|
||||
|
||||
unsigned int honda_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int toyota_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int subaru_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int chrysler_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int volkswagen_mqb_meb_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int xor_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int hkg_can_fd_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int fca_giorgio_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int body_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
unsigned int tesla_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
|
||||
#define DBC_ASSERT(condition, message) \
|
||||
do { \
|
||||
if (!(condition)) { \
|
||||
std::stringstream is; \
|
||||
is << "[" << dbc_name << ":" << line_num << "] " << message; \
|
||||
throw std::runtime_error(is.str()); \
|
||||
} \
|
||||
} while (false)
|
||||
|
||||
inline bool endswith(const std::string& str, const char* suffix) {
|
||||
return str.find(suffix, str.length() - strlen(suffix)) != std::string::npos;
|
||||
}
|
||||
|
||||
struct CanFrame {
|
||||
long src;
|
||||
uint32_t address;
|
||||
std::vector<uint8_t> dat;
|
||||
};
|
||||
|
||||
struct CanData {
|
||||
uint64_t nanos;
|
||||
std::vector<CanFrame> frames;
|
||||
};
|
||||
|
||||
class MessageState {
|
||||
public:
|
||||
std::string name;
|
||||
uint32_t address;
|
||||
unsigned int size;
|
||||
|
||||
std::vector<Signal> signals;
|
||||
std::vector<double> vals;
|
||||
std::vector<std::vector<double>> all_vals;
|
||||
|
||||
uint8_t counter;
|
||||
uint8_t counter_fail = 0;
|
||||
double frequency = 0.0f;
|
||||
uint64_t timeout_threshold = 0;
|
||||
std::deque<uint64_t> timestamps;
|
||||
|
||||
bool ignore_alive = false;
|
||||
bool ignore_checksum = false;
|
||||
bool ignore_counter = false;
|
||||
|
||||
bool valid(uint64_t current_nanos, bool bus_timeout) const;
|
||||
bool parse(uint64_t nanos, const std::vector<uint8_t> &dat);
|
||||
bool update_counter(int64_t v, int cnt_size);
|
||||
|
||||
uint64_t first_seen_nanos = 0;
|
||||
};
|
||||
|
||||
class CANParser {
|
||||
private:
|
||||
const int bus;
|
||||
const DBC *dbc = NULL;
|
||||
std::unordered_map<uint32_t, MessageState> message_states;
|
||||
|
||||
public:
|
||||
bool can_valid = false;
|
||||
bool bus_timeout = false;
|
||||
uint64_t last_nonempty_nanos = 0;
|
||||
int can_invalid_cnt = CAN_INVALID_CNT;
|
||||
|
||||
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);
|
||||
std::set<uint32_t> update(const std::vector<CanData> &can_data);
|
||||
MessageState *getMessageState(uint32_t address) { return &message_states.at(address); }
|
||||
|
||||
protected:
|
||||
void UpdateCans(const CanData &can, std::set<uint32_t> &updated_addresses);
|
||||
void UpdateValid(uint64_t nanos);
|
||||
};
|
||||
@@ -1,91 +0,0 @@
|
||||
# distutils: language = c++
|
||||
# cython: language_level=3
|
||||
|
||||
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
|
||||
from libcpp.deque cimport deque
|
||||
|
||||
|
||||
ctypedef unsigned int (*calc_checksum_type)(uint32_t, const Signal&, const vector[uint8_t] &)
|
||||
|
||||
cdef extern from "dbc.h":
|
||||
ctypedef enum SignalType:
|
||||
DEFAULT,
|
||||
COUNTER,
|
||||
HONDA_CHECKSUM,
|
||||
TOYOTA_CHECKSUM,
|
||||
BODY_CHECKSUM,
|
||||
VOLKSWAGEN_MQB_MEB_CHECKSUM,
|
||||
XOR_CHECKSUM,
|
||||
SUBARU_CHECKSUM,
|
||||
CHRYSLER_CHECKSUM
|
||||
HKG_CAN_FD_CHECKSUM,
|
||||
FCA_GIORGIO_CHECKSUM,
|
||||
TESLA_CHECKSUM,
|
||||
|
||||
cdef struct Signal:
|
||||
string name
|
||||
int start_bit, msb, lsb, size
|
||||
bool is_signed
|
||||
double factor, offset
|
||||
bool is_little_endian
|
||||
SignalType type
|
||||
calc_checksum_type calc_checksum
|
||||
|
||||
cdef struct Msg:
|
||||
string name
|
||||
uint32_t address
|
||||
unsigned int size
|
||||
vector[Signal] sigs
|
||||
|
||||
cdef struct Val:
|
||||
string name
|
||||
uint32_t address
|
||||
string def_val
|
||||
vector[Signal] sigs
|
||||
|
||||
cdef struct DBC:
|
||||
string name
|
||||
vector[Msg] msgs
|
||||
vector[Val] vals
|
||||
unordered_map[uint32_t, const Msg*] addr_to_msg
|
||||
unordered_map[string, const Msg*] name_to_msg
|
||||
|
||||
cdef struct SignalPackValue:
|
||||
string name
|
||||
double value
|
||||
|
||||
|
||||
cdef extern from "common.h":
|
||||
cdef const DBC* dbc_lookup(const string) except +
|
||||
|
||||
cdef cppclass MessageState:
|
||||
vector[Signal] signals
|
||||
vector[double] vals
|
||||
vector[vector[double]] all_vals
|
||||
deque[uint64_t] timestamps
|
||||
|
||||
cdef struct CanFrame:
|
||||
long src
|
||||
uint32_t address
|
||||
vector[uint8_t] dat
|
||||
|
||||
cdef struct CanData:
|
||||
uint64_t nanos
|
||||
vector[CanFrame] frames
|
||||
|
||||
cdef cppclass CANParser:
|
||||
bool can_valid
|
||||
bool bus_timeout
|
||||
CANParser(int, string, vector[pair[uint32_t, int]]) except + nogil
|
||||
set[uint32_t] update(vector[CanData]&) except + nogil
|
||||
MessageState *getMessageState(uint32_t address) nogil
|
||||
|
||||
cdef cppclass CANPacker:
|
||||
CANPacker(string) nogil
|
||||
vector[uint8_t] pack(uint32_t, vector[SignalPackValue]&) nogil
|
||||
@@ -1,253 +0,0 @@
|
||||
#include <algorithm>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <regex>
|
||||
#include <set>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <mutex>
|
||||
#include <iterator>
|
||||
#include <cstring>
|
||||
#include <clocale>
|
||||
|
||||
#include "opendbc/can/common.h"
|
||||
#include "opendbc/can/dbc.h"
|
||||
|
||||
std::regex bo_regexp(R"(^BO_ (\w+) (\w+) *: (\w+) (\w+))");
|
||||
std::regex sg_regexp(R"(^SG_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*))");
|
||||
std::regex sgm_regexp(R"(^SG_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*))");
|
||||
std::regex val_regexp(R"(VAL_ (\w+) (\w+) (\s*[-+]?[0-9]+\s+\".+?\"[^;]*))");
|
||||
std::regex val_split_regexp{R"([\"]+)"}; // split on "
|
||||
|
||||
inline bool startswith(const std::string& str, const char* prefix) {
|
||||
return str.find(prefix, 0) == 0;
|
||||
}
|
||||
|
||||
inline bool startswith(const std::string& str, std::initializer_list<const char*> prefix_list) {
|
||||
for (auto prefix : prefix_list) {
|
||||
if (startswith(str, prefix)) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
inline std::string& trim(std::string& s, const char* t = " \t\n\r\f\v") {
|
||||
s.erase(s.find_last_not_of(t) + 1);
|
||||
return s.erase(0, s.find_first_not_of(t));
|
||||
}
|
||||
|
||||
ChecksumState* get_checksum(const std::string& dbc_name) {
|
||||
ChecksumState* s = nullptr;
|
||||
if (startswith(dbc_name, {"honda_", "acura_"})) {
|
||||
s = new ChecksumState({4, 2, 3, 5, false, HONDA_CHECKSUM, &honda_checksum});
|
||||
} else if (startswith(dbc_name, {"toyota_", "lexus_"})) {
|
||||
s = new ChecksumState({8, -1, 7, -1, false, TOYOTA_CHECKSUM, &toyota_checksum});
|
||||
} else if (startswith(dbc_name, "hyundai_canfd_generated")) {
|
||||
s = new ChecksumState({16, -1, 0, -1, true, HKG_CAN_FD_CHECKSUM, &hkg_can_fd_checksum});
|
||||
} else if (startswith(dbc_name, {"vw_mqb", "vw_mqbevo", "vw_meb"})) {
|
||||
s = new ChecksumState({8, 4, 0, 0, true, VOLKSWAGEN_MQB_MEB_CHECKSUM, &volkswagen_mqb_meb_checksum});
|
||||
} else if (startswith(dbc_name, "vw_pq")) {
|
||||
s = new ChecksumState({8, 4, 0, -1, true, XOR_CHECKSUM, &xor_checksum});
|
||||
} else if (startswith(dbc_name, "subaru_global_")) {
|
||||
s = new ChecksumState({8, -1, 0, -1, true, SUBARU_CHECKSUM, &subaru_checksum});
|
||||
} else if (startswith(dbc_name, "chrysler_")) {
|
||||
s = new ChecksumState({8, -1, 7, -1, false, CHRYSLER_CHECKSUM, &chrysler_checksum});
|
||||
} else if (startswith(dbc_name, "fca_giorgio")) {
|
||||
s = new ChecksumState({8, -1, 7, -1, false, FCA_GIORGIO_CHECKSUM, &fca_giorgio_checksum});
|
||||
} else if (startswith(dbc_name, "comma_body")) {
|
||||
s = new ChecksumState({8, 4, 7, 3, false, BODY_CHECKSUM, &body_checksum});
|
||||
} else if (startswith(dbc_name, "tesla_model3_party")) {
|
||||
s = new ChecksumState({8, -1, 0, -1, true, TESLA_CHECKSUM, &tesla_checksum, &tesla_setup_signal});
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
void set_signal_type(Signal& s, ChecksumState* chk, const std::string& dbc_name, int line_num) {
|
||||
s.calc_checksum = nullptr;
|
||||
// FIXME: always assign COUNTER type without explicit ChecksumState
|
||||
if (chk) {
|
||||
if (chk->setup_signal) {
|
||||
chk->setup_signal(s, dbc_name, line_num);
|
||||
}
|
||||
|
||||
if (s.name == "CHECKSUM") {
|
||||
s.type = chk->checksum_type;
|
||||
s.calc_checksum = chk->calc_checksum;
|
||||
} else if (s.name == "COUNTER") {
|
||||
s.type = COUNTER;
|
||||
}
|
||||
|
||||
if (s.type > COUNTER) {
|
||||
DBC_ASSERT(chk->checksum_size == -1 || s.size == chk->checksum_size, s.name << " is not " << chk->checksum_size << " bits long");
|
||||
DBC_ASSERT(chk->checksum_start_bit == -1 || (s.start_bit % 8) == chk->checksum_start_bit, s.name << " starts at wrong bit");
|
||||
DBC_ASSERT(chk->little_endian == s.is_little_endian, s.name << " has wrong endianness");
|
||||
DBC_ASSERT(chk->calc_checksum != nullptr, "Checksum calculate function not supplied for " << s.name);
|
||||
} else if (s.type == COUNTER) {
|
||||
DBC_ASSERT(chk->counter_size == -1 || s.size == chk->counter_size, s.name << " is not " << chk->counter_size << " bits long");
|
||||
DBC_ASSERT(chk->counter_start_bit == -1 || (s.start_bit % 8) == chk->counter_start_bit, s.name << " starts at wrong bit");
|
||||
DBC_ASSERT(chk->little_endian == s.is_little_endian, s.name << " has wrong endianness");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, ChecksumState *checksum, bool allow_duplicate_msg_name) {
|
||||
uint32_t address = 0;
|
||||
std::set<uint32_t> address_set;
|
||||
std::set<std::string> msg_name_set;
|
||||
std::map<uint32_t, std::set<std::string>> signal_name_sets;
|
||||
std::map<uint32_t, std::vector<Signal>> signals;
|
||||
DBC* dbc = new DBC;
|
||||
dbc->name = dbc_name;
|
||||
std::setlocale(LC_NUMERIC, "C");
|
||||
|
||||
// used to find big endian LSB from MSB and size
|
||||
std::vector<int> be_bits;
|
||||
for (int i = 0; i < 64; i++) {
|
||||
for (int j = 7; j >= 0; j--) {
|
||||
be_bits.push_back(j + i * 8);
|
||||
}
|
||||
}
|
||||
|
||||
std::string line;
|
||||
int line_num = 0;
|
||||
std::smatch match;
|
||||
// TODO: see if we can speed up the regex statements in this loop, SG_ is specifically the slowest
|
||||
while (std::getline(stream, line)) {
|
||||
line = trim(line);
|
||||
line_num += 1;
|
||||
if (startswith(line, "BO_ ")) {
|
||||
// new group
|
||||
bool ret = std::regex_match(line, match, bo_regexp);
|
||||
DBC_ASSERT(ret, "bad BO: " << line);
|
||||
|
||||
Msg& msg = dbc->msgs.emplace_back();
|
||||
address = msg.address = std::stoul(match[1].str()); // could be hex
|
||||
msg.name = match[2].str();
|
||||
msg.size = std::stoul(match[3].str());
|
||||
|
||||
// check for duplicates
|
||||
DBC_ASSERT(address_set.find(address) == address_set.end(), "Duplicate message address: " << address << " (" << msg.name << ")");
|
||||
address_set.insert(address);
|
||||
|
||||
if (!allow_duplicate_msg_name) {
|
||||
DBC_ASSERT(msg_name_set.find(msg.name) == msg_name_set.end(), "Duplicate message name: " << msg.name);
|
||||
msg_name_set.insert(msg.name);
|
||||
}
|
||||
} else if (startswith(line, "SG_ ")) {
|
||||
// new signal
|
||||
int offset = 0;
|
||||
if (!std::regex_search(line, match, sg_regexp)) {
|
||||
bool ret = std::regex_search(line, match, sgm_regexp);
|
||||
DBC_ASSERT(ret, "bad SG: " << line);
|
||||
offset = 1;
|
||||
}
|
||||
Signal& sig = signals[address].emplace_back();
|
||||
sig.name = match[1].str();
|
||||
sig.start_bit = std::stoi(match[offset + 2].str());
|
||||
sig.size = std::stoi(match[offset + 3].str());
|
||||
sig.is_little_endian = std::stoi(match[offset + 4].str()) == 1;
|
||||
sig.is_signed = match[offset + 5].str() == "-";
|
||||
sig.factor = std::stod(match[offset + 6].str());
|
||||
sig.offset = std::stod(match[offset + 7].str());
|
||||
set_signal_type(sig, checksum, dbc_name, line_num);
|
||||
if (sig.is_little_endian) {
|
||||
sig.lsb = sig.start_bit;
|
||||
sig.msb = sig.start_bit + sig.size - 1;
|
||||
} else {
|
||||
auto it = find(be_bits.begin(), be_bits.end(), sig.start_bit);
|
||||
sig.lsb = be_bits[(it - be_bits.begin()) + sig.size - 1];
|
||||
sig.msb = sig.start_bit;
|
||||
}
|
||||
DBC_ASSERT(sig.lsb < (64 * 8) && sig.msb < (64 * 8), "Signal out of bounds: " << line);
|
||||
|
||||
// Check for duplicate signal names
|
||||
DBC_ASSERT(signal_name_sets[address].find(sig.name) == signal_name_sets[address].end(), "Duplicate signal name: " << sig.name);
|
||||
signal_name_sets[address].insert(sig.name);
|
||||
} else if (startswith(line, "VAL_ ")) {
|
||||
// new signal value/definition
|
||||
bool ret = std::regex_search(line, match, val_regexp);
|
||||
DBC_ASSERT(ret, "bad VAL: " << line);
|
||||
|
||||
auto& val = dbc->vals.emplace_back();
|
||||
val.address = std::stoul(match[1].str()); // could be hex
|
||||
val.name = match[2].str();
|
||||
|
||||
auto defvals = match[3].str();
|
||||
std::sregex_token_iterator it{defvals.begin(), defvals.end(), val_split_regexp, -1};
|
||||
// convert strings to UPPER_CASE_WITH_UNDERSCORES
|
||||
std::vector<std::string> words{it, {}};
|
||||
for (auto& w : words) {
|
||||
w = trim(w);
|
||||
std::transform(w.begin(), w.end(), w.begin(), ::toupper);
|
||||
std::replace(w.begin(), w.end(), ' ', '_');
|
||||
}
|
||||
// join string
|
||||
std::stringstream s;
|
||||
std::copy(words.begin(), words.end(), std::ostream_iterator<std::string>(s, " "));
|
||||
val.def_val = s.str();
|
||||
val.def_val = trim(val.def_val);
|
||||
}
|
||||
}
|
||||
|
||||
for (auto& m : dbc->msgs) {
|
||||
m.sigs = signals[m.address];
|
||||
dbc->addr_to_msg[m.address] = &m;
|
||||
dbc->name_to_msg[m.name] = &m;
|
||||
}
|
||||
for (auto& v : dbc->vals) {
|
||||
v.sigs = signals[v.address];
|
||||
}
|
||||
return dbc;
|
||||
}
|
||||
|
||||
DBC* dbc_parse(const std::string& dbc_path) {
|
||||
std::ifstream infile(dbc_path);
|
||||
if (!infile) return nullptr;
|
||||
|
||||
const std::string dbc_name = std::filesystem::path(dbc_path).filename();
|
||||
|
||||
std::unique_ptr<ChecksumState> checksum(get_checksum(dbc_name));
|
||||
return dbc_parse_from_stream(dbc_name, infile, checksum.get());
|
||||
}
|
||||
|
||||
const std::string get_dbc_root_path() {
|
||||
char *basedir = std::getenv("BASEDIR");
|
||||
if (basedir != NULL) {
|
||||
return std::string(basedir) + "/opendbc/dbc";
|
||||
} else {
|
||||
return DBC_FILE_PATH;
|
||||
}
|
||||
}
|
||||
|
||||
const DBC* dbc_lookup(const std::string& dbc_name) {
|
||||
static std::mutex lock;
|
||||
static std::map<std::string, DBC*> dbcs;
|
||||
|
||||
std::string dbc_file_path = dbc_name;
|
||||
if (!std::filesystem::exists(dbc_file_path)) {
|
||||
dbc_file_path = get_dbc_root_path() + "/" + dbc_name + ".dbc";
|
||||
}
|
||||
|
||||
std::unique_lock lk(lock);
|
||||
auto it = dbcs.find(dbc_name);
|
||||
if (it == dbcs.end()) {
|
||||
it = dbcs.insert(it, {dbc_name, dbc_parse(dbc_file_path)});
|
||||
}
|
||||
return it->second;
|
||||
}
|
||||
|
||||
std::vector<std::string> get_dbc_names() {
|
||||
static const std::string& dbc_file_path = get_dbc_root_path();
|
||||
std::vector<std::string> dbcs;
|
||||
for (std::filesystem::directory_iterator i(dbc_file_path), end; i != end; i++) {
|
||||
if (!is_directory(i->path())) {
|
||||
std::string filename = i->path().filename();
|
||||
if (!startswith(filename, "_") && endswith(filename, ".dbc")) {
|
||||
dbcs.push_back(filename.substr(0, filename.length() - 4));
|
||||
}
|
||||
}
|
||||
}
|
||||
return dbcs;
|
||||
}
|
||||
@@ -1,74 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
struct SignalPackValue {
|
||||
std::string name;
|
||||
double value;
|
||||
};
|
||||
|
||||
enum SignalType {
|
||||
DEFAULT,
|
||||
COUNTER,
|
||||
HONDA_CHECKSUM,
|
||||
TOYOTA_CHECKSUM,
|
||||
BODY_CHECKSUM,
|
||||
VOLKSWAGEN_MQB_MEB_CHECKSUM,
|
||||
XOR_CHECKSUM,
|
||||
SUBARU_CHECKSUM,
|
||||
CHRYSLER_CHECKSUM,
|
||||
HKG_CAN_FD_CHECKSUM,
|
||||
FCA_GIORGIO_CHECKSUM,
|
||||
TESLA_CHECKSUM,
|
||||
};
|
||||
|
||||
struct Signal {
|
||||
std::string name;
|
||||
int start_bit, msb, lsb, size;
|
||||
bool is_signed;
|
||||
double factor, offset;
|
||||
bool is_little_endian;
|
||||
SignalType type;
|
||||
unsigned int (*calc_checksum)(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
};
|
||||
|
||||
struct Msg {
|
||||
std::string name;
|
||||
uint32_t address;
|
||||
unsigned int size;
|
||||
std::vector<Signal> sigs;
|
||||
};
|
||||
|
||||
struct Val {
|
||||
std::string name;
|
||||
uint32_t address;
|
||||
std::string def_val;
|
||||
std::vector<Signal> sigs;
|
||||
};
|
||||
|
||||
struct DBC {
|
||||
std::string name;
|
||||
std::vector<Msg> msgs;
|
||||
std::vector<Val> vals;
|
||||
std::unordered_map<uint32_t, const Msg*> addr_to_msg;
|
||||
std::unordered_map<std::string, const Msg*> name_to_msg;
|
||||
};
|
||||
|
||||
typedef struct ChecksumState {
|
||||
int checksum_size;
|
||||
int counter_size;
|
||||
int checksum_start_bit;
|
||||
int counter_start_bit;
|
||||
bool little_endian;
|
||||
SignalType checksum_type;
|
||||
unsigned int (*calc_checksum)(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
|
||||
void (*setup_signal)(Signal &sig, const std::string& dbc_name, int line_num);
|
||||
} ChecksumState;
|
||||
|
||||
DBC* dbc_parse(const std::string& dbc_path);
|
||||
DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, ChecksumState *checksum = nullptr, bool allow_duplicate_msg_name=false);
|
||||
const DBC* dbc_lookup(const std::string& dbc_name);
|
||||
std::vector<std::string> get_dbc_names();
|
||||
@@ -1,26 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#ifdef SWAGLOG
|
||||
#include SWAGLOG
|
||||
#else
|
||||
|
||||
#define CLOUDLOG_DEBUG 10
|
||||
#define CLOUDLOG_INFO 20
|
||||
#define CLOUDLOG_WARNING 30
|
||||
#define CLOUDLOG_ERROR 40
|
||||
#define CLOUDLOG_CRITICAL 50
|
||||
|
||||
#define cloudlog(lvl, fmt, ...) printf(fmt "\n", ## __VA_ARGS__)
|
||||
#define cloudlog_rl(burst, millis, lvl, fmt, ...) printf(fmt "\n", ##__VA_ARGS__)
|
||||
|
||||
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
|
||||
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
|
||||
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
|
||||
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
|
||||
|
||||
#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
|
||||
#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
|
||||
#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
|
||||
#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
|
||||
|
||||
#endif
|
||||
@@ -44,12 +44,21 @@ class Msg:
|
||||
sigs: dict[str, Signal]
|
||||
|
||||
|
||||
@dataclass
|
||||
class Val:
|
||||
name: str
|
||||
address: int
|
||||
def_val: str
|
||||
sigs: dict[str, Signal] | None = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class DBC:
|
||||
name: str
|
||||
msgs: dict[int, Msg]
|
||||
addr_to_msg: dict[int, Msg]
|
||||
name_to_msg: dict[str, Msg]
|
||||
vals: list[Val]
|
||||
|
||||
|
||||
# ***** checksum functions *****
|
||||
@@ -309,6 +318,8 @@ def tesla_setup_signal(sig: Signal, dbc_name: str, line_num: int) -> None:
|
||||
BO_RE = re.compile(r"^BO_ (\w+) (\w+) *: (\w+) (\w+)")
|
||||
SG_RE = re.compile(r"^SG_ (\w+) : (\d+)\|(\d+)@(\d)([+-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[[0-9.+\-eE]+\|[0-9.+\-eE]+\] \".*\" .*")
|
||||
SGM_RE = re.compile(r"^SG_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d)([+-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[[0-9.+\-eE]+\|[0-9.+\-eE]+\] \".*\" .*")
|
||||
VAL_RE = re.compile(r"^VAL_ (\w+) (\w+) (.*);")
|
||||
VAL_SPLIT_RE = re.compile(r'["]+')
|
||||
|
||||
|
||||
def parse_dbc(path: str) -> DBC:
|
||||
@@ -321,6 +332,7 @@ def parse_dbc(path: str) -> DBC:
|
||||
msgs: dict[int, Msg] = {}
|
||||
addr_to_msg: dict[int, Msg] = {}
|
||||
name_to_msg: dict[str, Msg] = {}
|
||||
vals: list[Val] = []
|
||||
address = 0
|
||||
signals_temp: dict[int, dict[str, Signal]] = {}
|
||||
for line_num, line in enumerate(lines, 1):
|
||||
@@ -364,9 +376,20 @@ def parse_dbc(path: str) -> DBC:
|
||||
sig = Signal(sig_name, start_bit, msb, lsb, size, is_signed, factor, offset_val, is_little_endian)
|
||||
set_signal_type(sig, checksum_state, name, line_num)
|
||||
signals_temp[address][sig_name] = sig
|
||||
elif line.startswith("VAL_ "):
|
||||
m = VAL_RE.search(line)
|
||||
if not m:
|
||||
continue
|
||||
val_addr = int(m.group(1), 0)
|
||||
sgname = m.group(2)
|
||||
defs = m.group(3)
|
||||
words = [w.strip() for w in VAL_SPLIT_RE.split(defs) if w.strip()]
|
||||
words = [w.upper().replace(" ", "_") for w in words]
|
||||
val_def = " ".join(words).strip()
|
||||
vals.append(Val(sgname, val_addr, val_def))
|
||||
for addr, sigs in signals_temp.items():
|
||||
msgs[addr].sigs = sigs
|
||||
dbc = DBC(name, msgs, addr_to_msg, name_to_msg)
|
||||
dbc = DBC(name, msgs, addr_to_msg, name_to_msg, vals)
|
||||
return dbc
|
||||
|
||||
|
||||
@@ -421,9 +444,6 @@ def set_signal_type(sig: Signal, chk: ChecksumState | None, dbc_name: str, line_
|
||||
# ***** packer *****
|
||||
class CANPacker:
|
||||
def __init__(self, dbc_name: str):
|
||||
# Handle both string and bytes input (for compatibility with CANParser.dbc_name)
|
||||
if isinstance(dbc_name, bytes):
|
||||
dbc_name = dbc_name.decode("utf-8")
|
||||
dbc_path = dbc_name
|
||||
if not os.path.exists(dbc_path):
|
||||
dbc_path = os.path.join(os.path.dirname(__file__), "..", "dbc", dbc_name + ".dbc")
|
||||
|
||||
@@ -1,271 +0,0 @@
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <set>
|
||||
#include <stdexcept>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "opendbc/can/common.h"
|
||||
|
||||
|
||||
// ***** MessageState *****
|
||||
|
||||
int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
|
||||
int64_t ret = 0;
|
||||
|
||||
int i = sig.msb / 8;
|
||||
int bits = sig.size;
|
||||
while (i >= 0 && i < msg.size() && bits > 0) {
|
||||
int lsb = (int)(sig.lsb / 8) == i ? sig.lsb : i*8;
|
||||
int msb = (int)(sig.msb / 8) == i ? sig.msb : (i+1)*8 - 1;
|
||||
int size = msb - lsb + 1;
|
||||
|
||||
uint64_t d = (msg[i] >> (lsb - (i*8))) & ((1ULL << size) - 1);
|
||||
ret |= d << (bits - size);
|
||||
|
||||
bits -= size;
|
||||
i = sig.is_little_endian ? i-1 : i+1;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
|
||||
std::vector<double> tmp_vals(signals.size());
|
||||
bool checksum_failed = false;
|
||||
bool counter_failed = false;
|
||||
|
||||
if (first_seen_nanos == 0) {
|
||||
first_seen_nanos = nanos;
|
||||
}
|
||||
|
||||
for (int i = 0; i < signals.size(); i++) {
|
||||
const auto &sig = signals[i];
|
||||
|
||||
int64_t tmp = get_raw_value(dat, sig);
|
||||
if (sig.is_signed) {
|
||||
tmp -= ((tmp >> (sig.size-1)) & 0x1) ? (1ULL << sig.size) : 0;
|
||||
}
|
||||
|
||||
if (!ignore_checksum) {
|
||||
if (sig.calc_checksum != nullptr && sig.calc_checksum(address, sig, dat) != tmp) {
|
||||
checksum_failed = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!ignore_counter) {
|
||||
if (sig.type == SignalType::COUNTER && !update_counter(tmp, sig.size)) {
|
||||
counter_failed = true;
|
||||
}
|
||||
}
|
||||
|
||||
tmp_vals[i] = tmp * sig.factor + sig.offset;
|
||||
}
|
||||
|
||||
// only update values + timestamps if both checksum and counter are valid
|
||||
if (checksum_failed || counter_failed) {
|
||||
LOGE_100("0x%X message checks failed, checksum failed %d, counter failed %d", address, checksum_failed, counter_failed);
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < signals.size(); i++) {
|
||||
vals[i] = tmp_vals[i];
|
||||
all_vals[i].push_back(vals[i]);
|
||||
}
|
||||
timestamps.push_back(nanos);
|
||||
|
||||
const int max_buffer = 500; // allows 0.5s history for 1kHz (highest freq we've seen)
|
||||
while (timestamps.size() > max_buffer) {
|
||||
timestamps.pop_front();
|
||||
}
|
||||
|
||||
// learn message frequency
|
||||
if (frequency < 1e-5) {
|
||||
double dt = (timestamps.back() - timestamps.front())*1e-9;
|
||||
if ((timestamps.size() >= 3 && (dt > 1.0f)) || (timestamps.size() >= max_buffer)) {
|
||||
frequency = std::min(timestamps.size() / dt, (double)100.0f); // 100Hz max for checks
|
||||
timeout_threshold = (1000000000ULL / frequency) * 10; // timeout on 10x expected freq
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool MessageState::update_counter(int64_t cur_count, int cnt_size) {
|
||||
if (((counter + 1) & ((1 << cnt_size) - 1)) != cur_count) {
|
||||
counter_fail = std::min((int)counter_fail + 1, MAX_BAD_COUNTER);
|
||||
if (counter_fail > 1) {
|
||||
INFO("0x%X COUNTER FAIL #%d -- %d -> %d\n", address, counter_fail, counter, (int)cur_count);
|
||||
}
|
||||
} else if (counter_fail > 0) {
|
||||
counter_fail--;
|
||||
}
|
||||
counter = cur_count;
|
||||
return counter_fail < MAX_BAD_COUNTER;
|
||||
}
|
||||
|
||||
bool MessageState::valid(uint64_t current_nanos, bool bus_timeout) const {
|
||||
/*
|
||||
bad counters and checksums don't get added to timestamps, so those
|
||||
cases get caught here too.
|
||||
*/
|
||||
|
||||
if (ignore_alive) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const bool print = !bus_timeout && ((current_nanos - first_seen_nanos) > 7e9);
|
||||
if (timestamps.empty()) {
|
||||
if (print) LOGE_100("0x%X '%s' NOT SEEN", address, name.c_str());
|
||||
return false;
|
||||
}
|
||||
if (timeout_threshold > 0 && ((current_nanos - timestamps.back()) > timeout_threshold)) {
|
||||
if (print) LOGE_100("0x%X '%s' TIMED OUT", address, name.c_str());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// ***** CANParser *****
|
||||
|
||||
CANParser::CANParser(int abus, const std::string& dbc_name, const std::vector<std::pair<uint32_t, int>> &messages)
|
||||
: bus(abus) {
|
||||
dbc = dbc_lookup(dbc_name);
|
||||
assert(dbc);
|
||||
|
||||
for (const auto& [address, frequency] : messages) {
|
||||
// disallow duplicate message checks
|
||||
if (message_states.find(address) != message_states.end()) {
|
||||
std::stringstream is;
|
||||
is << "Duplicate Message Check: " << address;
|
||||
throw std::runtime_error(is.str());
|
||||
}
|
||||
|
||||
MessageState &state = message_states[address];
|
||||
state.address = address;
|
||||
// hack for signals whose frequencies vary more than 10x
|
||||
// TODO: figure out a good way to handle this without passing it in...
|
||||
if (frequency < 10 && frequency > 0) {
|
||||
state.frequency = frequency;
|
||||
state.timeout_threshold = (1000000000ULL / frequency) * 10; // timeout on 10x expected freq
|
||||
}
|
||||
state.ignore_alive = (frequency == 0);
|
||||
|
||||
const Msg *msg = dbc->addr_to_msg.at(address);
|
||||
state.name = msg->name;
|
||||
state.size = msg->size;
|
||||
assert(state.size <= 64); // max signal size is 64 bytes
|
||||
|
||||
// track all signals for this message
|
||||
state.signals = msg->sigs;
|
||||
state.vals.resize(msg->sigs.size());
|
||||
state.all_vals.resize(msg->sigs.size());
|
||||
}
|
||||
}
|
||||
|
||||
CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter)
|
||||
: bus(abus) {
|
||||
// Add all messages and signals
|
||||
|
||||
dbc = dbc_lookup(dbc_name);
|
||||
assert(dbc);
|
||||
|
||||
for (const auto& msg : dbc->msgs) {
|
||||
MessageState state = {
|
||||
.name = msg.name,
|
||||
.address = msg.address,
|
||||
.size = msg.size,
|
||||
.ignore_checksum = ignore_checksum,
|
||||
.ignore_counter = ignore_counter,
|
||||
};
|
||||
|
||||
for (const auto& sig : msg.sigs) {
|
||||
state.signals.push_back(sig);
|
||||
state.vals.push_back(0);
|
||||
state.all_vals.push_back({});
|
||||
}
|
||||
|
||||
message_states[state.address] = state;
|
||||
}
|
||||
}
|
||||
|
||||
std::set<uint32_t> CANParser::update(const std::vector<CanData> &can_data) {
|
||||
// Clear all_values
|
||||
for (auto &state : message_states) {
|
||||
for (auto &vals : state.second.all_vals) vals.clear();
|
||||
}
|
||||
|
||||
std::set<uint32_t> updated_addresses;
|
||||
for (const auto &c : can_data) {
|
||||
UpdateCans(c, updated_addresses);
|
||||
UpdateValid(c.nanos);
|
||||
}
|
||||
|
||||
return updated_addresses;
|
||||
}
|
||||
|
||||
void CANParser::UpdateCans(const CanData &can, std::set<uint32_t> &updated_addresses) {
|
||||
bool bus_empty = true;
|
||||
|
||||
for (const auto &frame : can.frames) {
|
||||
if (frame.src != bus) {
|
||||
continue;
|
||||
}
|
||||
bus_empty = false;
|
||||
|
||||
auto state_it = message_states.find(frame.address);
|
||||
if (state_it == message_states.end()) {
|
||||
continue;
|
||||
}
|
||||
if (frame.dat.size() > 64) {
|
||||
DEBUG("got message longer than 64 bytes: 0x%X %zu\n", frame.address, frame.dat.size());
|
||||
continue;
|
||||
}
|
||||
|
||||
// TODO: this actually triggers for some cars. fix and enable this
|
||||
//if (dat.size() != state_it->second.size) {
|
||||
// DEBUG("got message with unexpected length: expected %d, got %zu for %d", state_it->second.size, dat.size(), cmsg.getAddress());
|
||||
// continue;
|
||||
//}
|
||||
|
||||
if (state_it->second.parse(can.nanos, frame.dat)) {
|
||||
updated_addresses.insert(state_it->first);
|
||||
}
|
||||
}
|
||||
|
||||
// update bus timeout
|
||||
if (!bus_empty) {
|
||||
last_nonempty_nanos = can.nanos;
|
||||
}
|
||||
uint64_t bus_timeout_threshold = 500*1e6;
|
||||
for (const auto& kv : message_states) {
|
||||
const auto& state = kv.second;
|
||||
if (state.timeout_threshold > 0) {
|
||||
bus_timeout_threshold = std::min(bus_timeout_threshold, state.timeout_threshold);
|
||||
}
|
||||
}
|
||||
bus_timeout = (can.nanos - last_nonempty_nanos) > bus_timeout_threshold;
|
||||
}
|
||||
|
||||
void CANParser::UpdateValid(uint64_t nanos) {
|
||||
bool valid = true;
|
||||
bool counters_valid = true;
|
||||
|
||||
for (auto& kv : message_states) {
|
||||
const auto& state = kv.second;
|
||||
if (state.counter_fail >= MAX_BAD_COUNTER) {
|
||||
counters_valid = false;
|
||||
}
|
||||
if (!state.valid(nanos, bus_timeout)) {
|
||||
valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
can_invalid_cnt = valid ? 0 : std::min(can_invalid_cnt + 1, CAN_INVALID_CNT);
|
||||
can_valid = (can_invalid_cnt < CAN_INVALID_CNT) && counters_valid;
|
||||
}
|
||||
|
||||
252
opendbc/can/parser.py
Normal file
252
opendbc/can/parser.py
Normal file
@@ -0,0 +1,252 @@
|
||||
import os
|
||||
import numbers
|
||||
from collections import defaultdict, deque
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from opendbc.can.packer import DBC, DBC_CACHE, Signal, parse_dbc
|
||||
|
||||
|
||||
MAX_BAD_COUNTER = 5
|
||||
CAN_INVALID_CNT = 5
|
||||
|
||||
|
||||
def _get_dbc(dbc_name: str) -> DBC:
|
||||
dbc_path = dbc_name
|
||||
if not os.path.exists(dbc_path):
|
||||
dbc_path = os.path.join(os.path.dirname(__file__), "..", "dbc", dbc_name + ".dbc")
|
||||
if dbc_name in DBC_CACHE:
|
||||
return DBC_CACHE[dbc_name]
|
||||
try:
|
||||
dbc = parse_dbc(dbc_path)
|
||||
except FileNotFoundError as e:
|
||||
raise RuntimeError(f"DBC file not found: {dbc_path}") from e
|
||||
DBC_CACHE[dbc_name] = dbc
|
||||
return dbc
|
||||
|
||||
|
||||
def get_raw_value(dat: bytes | bytearray, sig: Signal) -> int:
|
||||
ret = 0
|
||||
i = sig.msb // 8
|
||||
bits = sig.size
|
||||
while 0 <= i < len(dat) and bits > 0:
|
||||
lsb = sig.lsb if (sig.lsb // 8) == i else i * 8
|
||||
msb = sig.msb if (sig.msb // 8) == i else (i + 1) * 8 - 1
|
||||
size = msb - lsb + 1
|
||||
d = (dat[i] >> (lsb - (i * 8))) & ((1 << size) - 1)
|
||||
ret |= d << (bits - size)
|
||||
bits -= size
|
||||
i = i - 1 if sig.is_little_endian else i + 1
|
||||
return ret
|
||||
|
||||
|
||||
@dataclass
|
||||
class MessageState:
|
||||
address: int
|
||||
name: str
|
||||
size: int
|
||||
signals: list[Signal]
|
||||
ignore_alive: bool = False
|
||||
ignore_checksum: bool = False
|
||||
ignore_counter: bool = False
|
||||
frequency: float = 0.0
|
||||
timeout_threshold: float = 0.0
|
||||
vals: list[float] = field(default_factory=list)
|
||||
all_vals: list[list[float]] = field(default_factory=list)
|
||||
timestamps: deque[int] = field(default_factory=deque)
|
||||
counter: int = 0
|
||||
counter_fail: int = 0
|
||||
first_seen_nanos: int = 0
|
||||
|
||||
def parse(self, nanos: int, dat: bytes) -> bool:
|
||||
tmp_vals: list[float] = [0.0] * len(self.signals)
|
||||
checksum_failed = False
|
||||
counter_failed = False
|
||||
|
||||
if self.first_seen_nanos == 0:
|
||||
self.first_seen_nanos = nanos
|
||||
|
||||
for i, sig in enumerate(self.signals):
|
||||
tmp = get_raw_value(dat, sig)
|
||||
if sig.is_signed:
|
||||
tmp -= ((tmp >> (sig.size - 1)) & 0x1) * (1 << sig.size)
|
||||
|
||||
if not self.ignore_checksum and sig.calc_checksum is not None:
|
||||
if sig.calc_checksum(self.address, sig, bytearray(dat)) != tmp:
|
||||
checksum_failed = True
|
||||
|
||||
if not self.ignore_counter and sig.type == 1: # COUNTER
|
||||
if not self.update_counter(tmp, sig.size):
|
||||
counter_failed = True
|
||||
|
||||
tmp_vals[i] = tmp * sig.factor + sig.offset
|
||||
|
||||
if checksum_failed or counter_failed:
|
||||
return False
|
||||
|
||||
if not self.vals:
|
||||
self.vals = [0.0] * len(self.signals)
|
||||
self.all_vals = [[] for _ in self.signals]
|
||||
|
||||
for i, v in enumerate(tmp_vals):
|
||||
self.vals[i] = v
|
||||
self.all_vals[i].append(v)
|
||||
|
||||
self.timestamps.append(nanos)
|
||||
max_buffer = 500
|
||||
while len(self.timestamps) > max_buffer:
|
||||
self.timestamps.popleft()
|
||||
|
||||
if self.frequency < 1e-5 and len(self.timestamps) >= 3:
|
||||
dt = (self.timestamps[-1] - self.timestamps[0]) * 1e-9
|
||||
if (dt > 1.0 or len(self.timestamps) >= max_buffer) and dt != 0:
|
||||
self.frequency = min(len(self.timestamps) / dt, 100.0)
|
||||
self.timeout_threshold = (1_000_000_000 / self.frequency) * 10
|
||||
return True
|
||||
|
||||
def update_counter(self, cur_count: int, cnt_size: int) -> bool:
|
||||
if ((self.counter + 1) & ((1 << cnt_size) - 1)) != cur_count:
|
||||
self.counter_fail = min(self.counter_fail + 1, MAX_BAD_COUNTER)
|
||||
elif self.counter_fail > 0:
|
||||
self.counter_fail -= 1
|
||||
self.counter = cur_count
|
||||
return self.counter_fail < MAX_BAD_COUNTER
|
||||
|
||||
def valid(self, current_nanos: int, bus_timeout: bool) -> bool:
|
||||
if self.ignore_alive:
|
||||
return True
|
||||
if not self.timestamps:
|
||||
return False
|
||||
if self.timeout_threshold > 0 and (current_nanos - self.timestamps[-1]) > self.timeout_threshold:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
class CANParser:
|
||||
def __init__(self, dbc_name: str, messages: list[tuple[str | int, int]], bus: int = 0):
|
||||
self.dbc_name: str = dbc_name
|
||||
self.bus: int = bus
|
||||
self.dbc: DBC | None = _get_dbc(dbc_name)
|
||||
if not self.dbc:
|
||||
raise RuntimeError(f"Can't find DBC: {dbc_name}")
|
||||
|
||||
self.vl: dict[int | str, dict[str, float]] = {}
|
||||
self.vl_all: dict[int | str, dict[str, list[float]]] = {}
|
||||
self.ts_nanos: dict[int | str, dict[str, int]] = {}
|
||||
self.addresses: set[int] = set()
|
||||
self.message_states: dict[int, MessageState] = {}
|
||||
|
||||
for name_or_addr, freq in messages:
|
||||
if isinstance(name_or_addr, numbers.Number):
|
||||
msg = self.dbc.addr_to_msg.get(int(name_or_addr))
|
||||
else:
|
||||
msg = self.dbc.name_to_msg.get(name_or_addr)
|
||||
if msg is None:
|
||||
raise RuntimeError(f"could not find message {name_or_addr!r} in DBC {dbc_name}")
|
||||
if msg.address in self.addresses:
|
||||
raise RuntimeError("Duplicate Message Check: %d" % msg.address)
|
||||
|
||||
self.addresses.add(msg.address)
|
||||
signal_names = list(msg.sigs.keys())
|
||||
self.vl[msg.address] = {s: 0.0 for s in signal_names}
|
||||
self.vl[msg.name] = self.vl[msg.address]
|
||||
self.vl_all[msg.address] = defaultdict(list)
|
||||
self.vl_all[msg.name] = self.vl_all[msg.address]
|
||||
self.ts_nanos[msg.address] = {s: 0 for s in signal_names}
|
||||
self.ts_nanos[msg.name] = self.ts_nanos[msg.address]
|
||||
|
||||
state = MessageState(
|
||||
address=msg.address,
|
||||
name=msg.name,
|
||||
size=msg.size,
|
||||
signals=list(msg.sigs.values()),
|
||||
ignore_alive=freq == 0,
|
||||
)
|
||||
if 0 < freq < 10:
|
||||
state.frequency = freq
|
||||
state.timeout_threshold = (1_000_000_000 / freq) * 10
|
||||
|
||||
self.message_states[msg.address] = state
|
||||
|
||||
self.can_valid: bool = False
|
||||
self.bus_timeout: bool = False
|
||||
self.can_invalid_cnt: int = CAN_INVALID_CNT
|
||||
self.last_nonempty_nanos: int = 0
|
||||
|
||||
def update_valid(self, nanos: int) -> None:
|
||||
valid = True
|
||||
counters_valid = True
|
||||
for state in self.message_states.values():
|
||||
if state.counter_fail >= MAX_BAD_COUNTER:
|
||||
counters_valid = False
|
||||
if not state.valid(nanos, self.bus_timeout):
|
||||
valid = False
|
||||
|
||||
self.can_invalid_cnt = 0 if valid else min(self.can_invalid_cnt + 1, CAN_INVALID_CNT)
|
||||
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT and counters_valid
|
||||
|
||||
def update_strings(self, strings, sendcan: bool = False):
|
||||
if strings and not isinstance(strings[0], list | tuple):
|
||||
strings = [strings]
|
||||
|
||||
for addr in self.addresses:
|
||||
for k in self.vl_all[addr]:
|
||||
self.vl_all[addr][k].clear()
|
||||
|
||||
updated_addrs: set[int] = set()
|
||||
for entry in strings:
|
||||
t = entry[0]
|
||||
frames = entry[1]
|
||||
bus_empty = True
|
||||
for address, dat, src in frames:
|
||||
if src != self.bus:
|
||||
continue
|
||||
bus_empty = False
|
||||
state = self.message_states.get(address)
|
||||
if state is None or len(dat) > 64:
|
||||
continue
|
||||
if state.parse(t, dat):
|
||||
updated_addrs.add(address)
|
||||
msgname = state.name
|
||||
for i, sig in enumerate(state.signals):
|
||||
val = state.vals[i]
|
||||
self.vl[address][sig.name] = val
|
||||
self.vl[msgname][sig.name] = val
|
||||
self.vl_all[address][sig.name] = state.all_vals[i]
|
||||
self.vl_all[msgname][sig.name] = state.all_vals[i]
|
||||
self.ts_nanos[address][sig.name] = state.timestamps[-1]
|
||||
self.ts_nanos[msgname][sig.name] = state.timestamps[-1]
|
||||
|
||||
if not bus_empty:
|
||||
self.last_nonempty_nanos = t
|
||||
bus_timeout_threshold = 500 * 1_000_000
|
||||
for st in self.message_states.values():
|
||||
if st.timeout_threshold > 0:
|
||||
bus_timeout_threshold = min(bus_timeout_threshold, st.timeout_threshold)
|
||||
self.bus_timeout = (t - self.last_nonempty_nanos) > bus_timeout_threshold
|
||||
self.update_valid(t)
|
||||
|
||||
return updated_addrs
|
||||
|
||||
|
||||
class CANDefine:
|
||||
def __init__(self, dbc_name: str):
|
||||
self.dbc_name = dbc_name
|
||||
self.dbc = _get_dbc(dbc_name)
|
||||
if not self.dbc:
|
||||
raise RuntimeError(f"Can't find DBC: '{dbc_name}'")
|
||||
|
||||
dv = defaultdict(dict)
|
||||
for val in self.dbc.vals:
|
||||
sgname = val.name
|
||||
address = val.address
|
||||
msg = self.dbc.addr_to_msg.get(address)
|
||||
if msg is None:
|
||||
raise KeyError(address)
|
||||
msgname = msg.name
|
||||
parts = val.def_val.split()
|
||||
values = [int(v) for v in parts[::2]]
|
||||
defs = parts[1::2]
|
||||
dv[address][sgname] = dict(zip(values, defs, strict=True))
|
||||
dv[msgname][sgname] = dv[address][sgname]
|
||||
|
||||
self.dv = dict(dv)
|
||||
@@ -1,176 +0,0 @@
|
||||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
from libcpp.pair cimport pair
|
||||
from libcpp.string cimport string
|
||||
from libcpp.vector cimport vector
|
||||
from libc.stdint cimport uint32_t, int
|
||||
|
||||
from .common cimport CANParser as cpp_CANParser
|
||||
from .common cimport dbc_lookup, Msg, DBC, CanData
|
||||
|
||||
import numbers
|
||||
from collections import defaultdict
|
||||
|
||||
|
||||
cdef class CANParser:
|
||||
cdef:
|
||||
cpp_CANParser *can
|
||||
const DBC *dbc
|
||||
set addresses
|
||||
|
||||
cdef readonly:
|
||||
dict vl
|
||||
dict vl_all
|
||||
dict ts_nanos
|
||||
string dbc_name
|
||||
uint32_t bus
|
||||
|
||||
def __init__(self, dbc_name, messages, bus=0):
|
||||
self.dbc_name = dbc_name
|
||||
self.bus = bus
|
||||
self.dbc = dbc_lookup(dbc_name)
|
||||
if not self.dbc:
|
||||
raise RuntimeError(f"Can't find DBC: {dbc_name}")
|
||||
|
||||
self.vl = {}
|
||||
self.vl_all = {}
|
||||
self.ts_nanos = {}
|
||||
self.addresses = set()
|
||||
|
||||
# Convert message names into addresses and check existence in DBC
|
||||
cdef vector[pair[uint32_t, int]] message_v
|
||||
for i in range(len(messages)):
|
||||
c = messages[i]
|
||||
try:
|
||||
m = self.dbc.addr_to_msg.at(c[0]) if isinstance(c[0], numbers.Number) else self.dbc.name_to_msg.at(c[0])
|
||||
except IndexError:
|
||||
raise RuntimeError(f"could not find message {repr(c[0])} in DBC {self.dbc_name}")
|
||||
|
||||
address = m.address
|
||||
message_v.push_back((address, c[1]))
|
||||
self.addresses.add(address)
|
||||
|
||||
name = m.name.decode("utf8")
|
||||
signal_names = [sig.name.decode("utf-8") for sig in (<Msg*>m).sigs]
|
||||
|
||||
self.vl[address] = {name: 0.0 for name in signal_names}
|
||||
self.vl[name] = self.vl[address]
|
||||
self.vl_all[address] = defaultdict(list)
|
||||
self.vl_all[name] = self.vl_all[address]
|
||||
self.ts_nanos[address] = {name: 0.0 for name in signal_names}
|
||||
self.ts_nanos[name] = self.ts_nanos[address]
|
||||
|
||||
cdef string cpp_dbc_name
|
||||
if isinstance(dbc_name, str):
|
||||
cpp_dbc_name = (<str>dbc_name).encode("utf-8")
|
||||
else:
|
||||
cpp_dbc_name = dbc_name # Assume bytes
|
||||
cdef int cpp_bus = bus
|
||||
with nogil:
|
||||
self.can = new cpp_CANParser(cpp_bus, cpp_dbc_name, message_v)
|
||||
|
||||
def __dealloc__(self):
|
||||
if self.can:
|
||||
with nogil:
|
||||
del self.can
|
||||
|
||||
def update_strings(self, strings, sendcan=False):
|
||||
# input format:
|
||||
# [nanos, [[address, data, src], ...]]
|
||||
# [[nanos, [[address, data, src], ...], ...]]
|
||||
for address in self.addresses:
|
||||
self.vl_all[address].clear()
|
||||
|
||||
cdef vector[CanData] can_data_array
|
||||
|
||||
try:
|
||||
if len(strings) and not isinstance(strings[0], (list, tuple)):
|
||||
strings = [strings]
|
||||
|
||||
can_data_array.reserve(len(strings))
|
||||
for s in strings:
|
||||
can_data = &(can_data_array.emplace_back())
|
||||
can_data.nanos = s[0]
|
||||
can_data.frames.reserve(len(s[1]))
|
||||
for address, dat, src in s[1]:
|
||||
source_bus = <uint32_t>src
|
||||
if source_bus == self.bus:
|
||||
frame = &(can_data.frames.emplace_back())
|
||||
frame.address = address
|
||||
frame.dat = dat
|
||||
frame.src = source_bus
|
||||
except TypeError:
|
||||
raise RuntimeError("invalid parameter")
|
||||
|
||||
with nogil:
|
||||
updated_addrs = self.can.update(can_data_array)
|
||||
|
||||
for addr in updated_addrs:
|
||||
vl = self.vl[addr]
|
||||
vl_all = self.vl_all[addr]
|
||||
ts_nanos = self.ts_nanos[addr]
|
||||
|
||||
with nogil:
|
||||
state = self.can.getMessageState(addr)
|
||||
for i in range(state.signals.size()):
|
||||
name = <unicode>state.signals[i].name
|
||||
vl[name] = state.vals[i]
|
||||
vl_all[name] = state.all_vals[i]
|
||||
ts_nanos[name] = state.timestamps.back() if not state.timestamps.empty() else 0
|
||||
|
||||
return updated_addrs
|
||||
|
||||
@property
|
||||
def can_valid(self):
|
||||
cdef bint valid
|
||||
with nogil:
|
||||
valid = self.can.can_valid
|
||||
return valid
|
||||
|
||||
@property
|
||||
def bus_timeout(self):
|
||||
cdef bint timeout
|
||||
with nogil:
|
||||
timeout = self.can.bus_timeout
|
||||
return timeout
|
||||
|
||||
|
||||
cdef class CANDefine():
|
||||
cdef:
|
||||
const DBC *dbc
|
||||
|
||||
cdef public:
|
||||
dict dv
|
||||
string dbc_name
|
||||
|
||||
def __init__(self, dbc_name):
|
||||
self.dbc_name = dbc_name
|
||||
self.dbc = dbc_lookup(dbc_name)
|
||||
if not self.dbc:
|
||||
raise RuntimeError(f"Can't find DBC: '{dbc_name}'")
|
||||
|
||||
dv = defaultdict(dict)
|
||||
|
||||
for i in range(self.dbc[0].vals.size()):
|
||||
val = self.dbc[0].vals[i]
|
||||
|
||||
sgname = val.name.decode("utf8")
|
||||
def_val = val.def_val.decode("utf8")
|
||||
address = val.address
|
||||
try:
|
||||
m = self.dbc.addr_to_msg.at(address)
|
||||
except IndexError:
|
||||
raise KeyError(address)
|
||||
msgname = m.name.decode("utf-8")
|
||||
|
||||
# separate definition/value pairs
|
||||
def_val = def_val.split()
|
||||
values = [int(v) for v in def_val[::2]]
|
||||
defs = def_val[1::2]
|
||||
|
||||
# two ways to lookup: address or msg name
|
||||
dv[address][sgname] = dict(zip(values, defs))
|
||||
dv[msgname][sgname] = dv[address][sgname]
|
||||
|
||||
self.dv = dict(dv)
|
||||
@@ -19,7 +19,7 @@ class TestCanParserPackerExceptions:
|
||||
CANDefine(TEST_DBC)
|
||||
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
with pytest.raises(RuntimeError):
|
||||
with pytest.raises(IndexError):
|
||||
parser.update_strings([b''])
|
||||
|
||||
# Everything is supposed to work below
|
||||
|
||||
@@ -9,16 +9,12 @@ requires-python = ">=3.9,<3.13" # pycapnp doesn't work with 3.13
|
||||
|
||||
urls = { "homepage" = "https://github.com/commaai/opendbc" }
|
||||
|
||||
# setuptools includes distutils which is needed by Cython.
|
||||
# distutils was removed in python3.12 from the standard library.
|
||||
dependencies = [
|
||||
"scons",
|
||||
"numpy",
|
||||
"Cython",
|
||||
"crcmod",
|
||||
"tqdm",
|
||||
"pycapnp",
|
||||
"setuptools",
|
||||
"pycryptodome",
|
||||
]
|
||||
|
||||
@@ -40,7 +36,6 @@ testing = [
|
||||
"ty",
|
||||
"lefthook",
|
||||
"cpplint",
|
||||
"cython-lint",
|
||||
"codespell",
|
||||
]
|
||||
docs = [
|
||||
@@ -69,10 +64,6 @@ ignore-words-list = "alo,ba,bu,deque,hda,grey,arange"
|
||||
builtin = "clear,rare,informal,code,names,en-GB_to_en-US"
|
||||
check-hidden = true
|
||||
|
||||
[tool.cython-lint]
|
||||
max-line-length = 120
|
||||
ignore = ["E111", "E114"]
|
||||
|
||||
[tool.mypy]
|
||||
# helpful warnings
|
||||
warn_redundant_casts=true
|
||||
|
||||
@@ -1,72 +0,0 @@
|
||||
import re
|
||||
import SCons
|
||||
from SCons.Action import Action
|
||||
from SCons.Scanner import Scanner
|
||||
|
||||
pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M)
|
||||
pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M)
|
||||
cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M)
|
||||
|
||||
|
||||
def pyx_scan(node, env, path, arg=None):
|
||||
contents = node.get_text_contents()
|
||||
|
||||
# from <module> cimport ...
|
||||
matches = pyx_from_import_re.findall(contents)
|
||||
# cimport <module>
|
||||
matches += pyx_import_re.findall(contents)
|
||||
|
||||
# Modules can be either .pxd or .pyx files
|
||||
files = [m.replace('.', '/') + '.pxd' for m in matches]
|
||||
files += [m.replace('.', '/') + '.pyx' for m in matches]
|
||||
|
||||
# cdef extern from <file>
|
||||
files += cdef_import_re.findall(contents)
|
||||
|
||||
# Handle relative imports
|
||||
cur_dir = str(node.get_dir())
|
||||
files = [cur_dir + f if f.startswith('/') else f for f in files]
|
||||
|
||||
# Filter out non-existing files (probably system imports)
|
||||
files = [f for f in files if env.File(f).exists()]
|
||||
return env.File(files)
|
||||
|
||||
|
||||
pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True)
|
||||
cythonAction = Action("$CYTHONCOM")
|
||||
|
||||
|
||||
def create_builder(env):
|
||||
try:
|
||||
cython = env['BUILDERS']['Cython']
|
||||
except KeyError:
|
||||
cython = SCons.Builder.Builder(
|
||||
action=cythonAction,
|
||||
emitter={},
|
||||
suffix=cython_suffix_emitter,
|
||||
single_source=1
|
||||
)
|
||||
env.Append(SCANNERS=pyxscanner)
|
||||
env['BUILDERS']['Cython'] = cython
|
||||
return cython
|
||||
|
||||
def cython_suffix_emitter(env, source):
|
||||
return "$CYTHONCFILESUFFIX"
|
||||
|
||||
def generate(env):
|
||||
env["CYTHON"] = "cythonize"
|
||||
env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE"
|
||||
env["CYTHONCFILESUFFIX"] = ".cpp"
|
||||
|
||||
c_file, _ = SCons.Tool.createCFileBuilders(env)
|
||||
|
||||
c_file.suffix['.pyx'] = cython_suffix_emitter
|
||||
c_file.add_action('.pyx', cythonAction)
|
||||
|
||||
c_file.suffix['.py'] = cython_suffix_emitter
|
||||
c_file.add_action('.py', cythonAction)
|
||||
|
||||
create_builder(env)
|
||||
|
||||
def exists(env):
|
||||
return True
|
||||
Reference in New Issue
Block a user