IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Prebuilt Release @ 27f668a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-03 18:23:24 -05:00
commit b073c5182b
2554 changed files with 679696 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
artifacts/package_runtime/iqdbc/__init__.py Normal file
View File

@@ -0,0 +1,6 @@
import os
DBC_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'dbc')
# -I include path for e.g. "#include <iqdbc/safety/safety.h>"
INCLUDE_PATH = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))

8
artifacts/package_runtime/iqdbc/can/__init__.py Normal file
View File

@@ -0,0 +1,8 @@
from iqdbc.can.packer import CANPacker
from iqdbc.can.parser import CANParser, CANDefine
__all__ = [
"CANDefine",
"CANParser",
"CANPacker",
]

217
artifacts/package_runtime/iqdbc/can/dbc.py Normal file
View File

@@ -0,0 +1,217 @@
import re
import os
from collections.abc import Callable
from dataclasses import dataclass
from functools import cache
from iqdbc import DBC_PATH
# TODO: these should just be passed in along with the DBC file
from iqdbc.car.honda.hondacan import honda_checksum
from iqdbc.car.toyota.toyotacan import toyota_checksum
from iqdbc.car.subaru.subarucan import subaru_checksum
from iqdbc.car.chrysler.chryslercan import chrysler_checksum, fca_giorgio_checksum
from iqdbc.car.hyundai.hyundaicanfd import hkg_can_fd_checksum
from iqdbc.car.volkswagen.mlbcan import volkswagen_mlb_checksum
from iqdbc.car.volkswagen.mqbcan import volkswagen_mqb_meb_checksum, volkswagen_mqb_meb_gen2_checksum, xor_checksum
from iqdbc.car.tesla.teslacan import tesla_checksum
from iqdbc.car.body.bodycan import body_checksum
from iqdbc.car.psa.psacan import psa_checksum
class SignalType:
DEFAULT = 0
COUNTER = 1
HONDA_CHECKSUM = 2
TOYOTA_CHECKSUM = 3
BODY_CHECKSUM = 4
VOLKSWAGEN_MQB_MEB_CHECKSUM = 5
XOR_CHECKSUM = 6
SUBARU_CHECKSUM = 7
CHRYSLER_CHECKSUM = 8
HKG_CAN_FD_CHECKSUM = 9
FCA_GIORGIO_CHECKSUM = 10
TESLA_CHECKSUM = 11
PSA_CHECKSUM = 12
VOLKSWAGEN_MLB_CHECKSUM = 13
VOLKSWAGEN_MQB_MEB_GEN2_CHECKSUM = 14
@dataclass
class Signal:
name: str
start_bit: int
msb: int
lsb: int
size: int
is_signed: bool
factor: float
offset: float
is_little_endian: bool
type: int = SignalType.DEFAULT
calc_checksum: 'Callable[[int, Signal, bytearray], int] | None' = None
@dataclass
class Msg:
name: str
address: int
size: int
sigs: dict[str, Signal]
@dataclass
class Val:
name: str
address: int
def_val: str
sigs: dict[str, Signal] | None = 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'["]+')
@cache
class DBC:
def __init__(self, name: str):
dbc_path = name
if not os.path.exists(dbc_path):
dbc_path = os.path.join(DBC_PATH, name + ".dbc")
self._parse(dbc_path)
def _parse(self, path: str):
self.name = os.path.basename(path).replace(".dbc", "")
with open(path) as f:
lines = f.readlines()
checksum_state = get_checksum_state(self.name)
be_bits = [j + i * 8 for i in range(64) for j in range(7, -1, -1)]
self.msgs: dict[int, Msg] = {}
self.addr_to_msg: dict[int, Msg] = {}
self.name_to_msg: dict[str, Msg] = {}
self.vals: list[Val] = []
address = 0
signals_temp: dict[int, dict[str, Signal]] = {}
for line_num, line in enumerate(lines, 1):
line = line.strip()
if line.startswith("BO_ "):
m = BO_RE.match(line)
if not m:
continue
address = int(m.group(1), 0)
msg_name = m.group(2)
size = int(m.group(3), 0)
sigs = {}
self.msgs[address] = Msg(msg_name, address, size, sigs)
self.addr_to_msg[address] = self.msgs[address]
self.name_to_msg[msg_name] = self.msgs[address]
signals_temp[address] = sigs
elif line.startswith("SG_ "):
m = SG_RE.search(line)
offset = 0
if not m:
m = SGM_RE.search(line)
if not m:
continue
offset = 1
sig_name = m.group(1)
start_bit = int(m.group(2 + offset))
size = int(m.group(3 + offset))
is_little_endian = m.group(4 + offset) == "1"
is_signed = m.group(5 + offset) == "-"
factor = float(m.group(6 + offset))
offset_val = float(m.group(7 + offset))
if is_little_endian:
lsb = start_bit
msb = start_bit + size - 1
else:
idx = be_bits.index(start_bit)
lsb = be_bits[idx + size - 1]
msb = start_bit
sig = Signal(sig_name, start_bit, msb, lsb, size, is_signed, factor, offset_val, is_little_endian)
set_signal_type(sig, checksum_state, self.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()
self.vals.append(Val(sgname, val_addr, val_def))
for addr, sigs in signals_temp.items():
self.msgs[addr].sigs = sigs
# ***** checksum functions *****
def tesla_setup_signal(sig: Signal, dbc_name: str, line_num: int) -> None:
if sig.name.endswith("Counter"):
sig.type = SignalType.COUNTER
elif sig.name.endswith("Checksum"):
sig.type = SignalType.TESLA_CHECKSUM
sig.calc_checksum = tesla_checksum
@dataclass
class ChecksumState:
checksum_size: int
counter_size: int
checksum_start_bit: int
counter_start_bit: int
little_endian: bool
checksum_type: int
calc_checksum: Callable[[int, Signal, bytearray], int] | None
setup_signal: Callable[[Signal, str, int], None] | None = None
def get_checksum_state(dbc_name: str) -> ChecksumState | None:
if dbc_name.startswith(("honda_", "acura_")):
return ChecksumState(4, 2, 3, 5, False, SignalType.HONDA_CHECKSUM, honda_checksum)
elif dbc_name.startswith(("toyota_", "lexus_")):
return ChecksumState(8, -1, 7, -1, False, SignalType.TOYOTA_CHECKSUM, toyota_checksum)
elif dbc_name.startswith("hyundai_canfd_generated"):
return ChecksumState(16, -1, 0, -1, True, SignalType.HKG_CAN_FD_CHECKSUM, hkg_can_fd_checksum)
elif dbc_name.startswith("vw_meb_2024"):
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_GEN2_CHECKSUM, volkswagen_mqb_meb_gen2_checksum)
elif dbc_name.startswith(("vw_mqb", "vw_mqbevo", "vw_meb")):
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_mqb_meb_checksum)
elif dbc_name.startswith("vw_mlb"):
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MLB_CHECKSUM, volkswagen_mlb_checksum)
elif dbc_name.startswith("vw_pq"):
return ChecksumState(8, 4, 0, -1, True, SignalType.XOR_CHECKSUM, xor_checksum)
elif dbc_name.startswith("subaru_global_"):
return ChecksumState(8, -1, 0, -1, True, SignalType.SUBARU_CHECKSUM, subaru_checksum)
elif dbc_name.startswith("chrysler_"):
return ChecksumState(8, -1, 7, -1, False, SignalType.CHRYSLER_CHECKSUM, chrysler_checksum)
elif dbc_name.startswith("fca_giorgio"):
return ChecksumState(8, -1, 7, -1, False, SignalType.FCA_GIORGIO_CHECKSUM, fca_giorgio_checksum)
elif dbc_name.startswith("comma_body"):
return ChecksumState(8, 4, 7, 3, False, SignalType.BODY_CHECKSUM, body_checksum)
elif dbc_name.startswith(("tesla_model3_party", "tesla_model3_vehicle")):
return ChecksumState(8, -1, 0, -1, True, SignalType.TESLA_CHECKSUM, tesla_checksum, tesla_setup_signal)
elif dbc_name.startswith("psa_"):
return ChecksumState(4, 4, 7, 3, False, SignalType.PSA_CHECKSUM, psa_checksum)
return None
def set_signal_type(sig: Signal, chk: ChecksumState | None, dbc_name: str, line_num: int) -> None:
sig.calc_checksum = None
if chk:
if chk.setup_signal:
chk.setup_signal(sig, dbc_name, line_num)
if sig.name == "CHECKSUM":
sig.type = chk.checksum_type
sig.calc_checksum = chk.calc_checksum
elif sig.name == "COUNTER":
sig.type = SignalType.COUNTER

72
artifacts/package_runtime/iqdbc/can/packer.py Normal file
View File

@@ -0,0 +1,72 @@
import math
from iqdbc.car.carlog import carlog
from iqdbc.can.dbc import DBC, Signal, SignalType
class CANPacker:
def __init__(self, dbc_name: str):
self.dbc = DBC(dbc_name)
self.counters: dict[int, int] = {}
def pack(self, address: int, values: dict[str, float]) -> bytearray:
msg = self.dbc.addr_to_msg.get(address)
if msg is None:
carlog.error(f"msg not found for {address=}")
return bytearray()
dat = bytearray(msg.size)
counter_set = False
for name, value in values.items():
sig = msg.sigs.get(name)
if sig is None:
carlog.error(f"unknown signal {name=} in {msg.name}")
continue
ival = int(math.floor((value - sig.offset) / sig.factor + 0.5))
if ival < 0:
ival = (1 << sig.size) + ival
set_value(dat, sig, ival)
if sig.type == SignalType.COUNTER or sig.name == "COUNTER":
self.counters[address] = int(value)
counter_set = True
sig_counter = next((s for s in msg.sigs.values() if s.type == SignalType.COUNTER or s.name == "COUNTER"), None)
if sig_counter and not counter_set:
if address not in self.counters:
self.counters[address] = 0
set_value(dat, sig_counter, self.counters[address])
self.counters[address] = (self.counters[address] + 1) % (1 << sig_counter.size)
sig_checksum = next((s for s in msg.sigs.values() if s.type > SignalType.COUNTER), None)
if sig_checksum and sig_checksum.calc_checksum:
checksum = sig_checksum.calc_checksum(address, sig_checksum, dat)
set_value(dat, sig_checksum, checksum)
return dat
def make_can_msg(self, name_or_addr, bus: int, values: dict[str, float], rx_counter: int | None = None):
if isinstance(name_or_addr, int):
addr = name_or_addr
else:
msg = self.dbc.name_to_msg.get(name_or_addr)
if msg is None:
carlog.error(f"msg not found for {name_or_addr=}")
return 0, b'', bus
addr = msg.address
pack_values = values if rx_counter is None else values | {"COUNTER": rx_counter}
dat = self.pack(addr, pack_values)
if len(dat) == 0:
return 0, b'', bus
return addr, bytes(dat), bus
def set_value(msg: bytearray, sig: Signal, ival: int) -> None:
i = sig.lsb // 8
bits = sig.size
if sig.size < 64:
ival &= (1 << sig.size) - 1
while 0 <= i < len(msg) and bits > 0:
shift = sig.lsb % 8 if (sig.lsb // 8) == i else 0
size = min(bits, 8 - shift)
mask = ((1 << size) - 1) << shift
msg[i] &= ~mask
msg[i] |= (ival & ((1 << size) - 1)) << shift
bits -= size
ival >>= size
i = i + 1 if sig.is_little_endian else i - 1

285
artifacts/package_runtime/iqdbc/can/parser.py Normal file
View File

@@ -0,0 +1,285 @@
import math
import numbers
import time
from collections import defaultdict, deque
from dataclasses import dataclass, field
from iqdbc.car.carlog import carlog
from iqdbc.can.dbc import DBC, Signal
MAX_BAD_COUNTER = 5
CAN_INVALID_CNT = 5
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 = 1e5 # default to 1Hz threshold
vals: list[float] = field(default_factory=list)
all_vals: list[list[float]] = field(default_factory=list)
timestamps: deque[int] = field(default_factory=lambda: deque(maxlen=500))
counter: int = 0
counter_fail: int = 0
first_seen_nanos: int = 0
last_warning_log_nanos: int = 0
def rate_limited_log(self, last_update_nanos: int, msg: str) -> None:
if (last_update_nanos - self.last_warning_log_nanos) >= 1_000_000_000:
carlog.warning(f"CANParser: {hex(self.address)} {self.name} {msg}")
self.last_warning_log_nanos = last_update_nanos
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:
expected_checksum = sig.calc_checksum(self.address, sig, bytearray(dat))
if tmp != expected_checksum:
checksum_failed = True
self.rate_limited_log(nanos, f"checksum failed: received {hex(tmp)}, calculated {hex(expected_checksum)}")
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
# must have good counter and checksum to update data
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)
if self.frequency < 1e-5 and len(self.timestamps) >= 3:
dt = (self.timestamps[-1] - self.timestamps[0]) * 1e-9
if (dt > 1.0 or (self.timestamps.maxlen is not None and len(self.timestamps) >= self.timestamps.maxlen)) 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 (current_nanos - self.timestamps[-1]) > self.timeout_threshold:
return False
return True
class VLDict(dict):
def __init__(self, parser):
super().__init__()
self.parser = parser
def __getitem__(self, key):
if key not in self:
self.parser._add_message(key)
return super().__getitem__(key)
class CANParser:
def __init__(self, dbc_name: str, messages: list[tuple[str | int, int]], bus: int):
self.dbc_name: str = dbc_name
self.bus: int = bus
self.dbc: DBC = DBC(dbc_name)
self.vl: dict[int | str, dict[str, float]] = VLDict(self)
self.vl_all: dict[int | str, dict[str, list[float]]] = {}
self.ts_nanos: dict[int | str, dict[str, int]] = {}
self.dat: dict[int | str, bytes] = {}
self.addresses: set[int] = set()
self.seen_addresses: set[int] = set()
self.enable_capture: bool = True
self.controls_ready: bool = False
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._add_message(name_or_addr, freq)
self.can_invalid_cnt: int = CAN_INVALID_CNT
self.last_nonempty_nanos: int = 0
self._last_update_nanos: int = 0
def _add_message(self, name_or_addr: str | int, freq: int | None = None, ignore_counter: bool = False) -> None:
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)
assert msg is not None
assert msg.address not in self.addresses
self.addresses.add(msg.address)
signal_names = list(msg.sigs.keys())
signals_dict = {s: 0.0 for s in signal_names}
dict.__setitem__(self.vl, msg.address, signals_dict)
dict.__setitem__(self.vl, msg.name, signals_dict)
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 is not None and math.isnan(freq),
ignore_counter=ignore_counter,
)
state.first_seen_nanos = time.monotonic_ns()
if freq is not None and freq > 0:
state.frequency = freq
else:
# if frequency not specified, assume 1Hz until we learn it
freq = 1
state.timeout_threshold = (1_000_000_000 / freq) * 10
self.message_states[msg.address] = state
@property
def bus_timeout(self) -> bool:
ignore_alive = all(s.ignore_alive for s in self.message_states.values())
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)
return ((self._last_update_nanos - self.last_nonempty_nanos) > bus_timeout_threshold) and not ignore_alive
@property
def can_valid(self) -> bool:
valid = True
counters_valid = True
bus_timeout = self.bus_timeout
for state in self.message_states.values():
if state.counter_fail >= MAX_BAD_COUNTER:
counters_valid = False
state.rate_limited_log(self._last_update_nanos, f"counter invalid, {state.counter_fail=} {MAX_BAD_COUNTER=}")
if not state.valid(self._last_update_nanos, bus_timeout):
valid = False
state.rate_limited_log(self._last_update_nanos, "not valid (timeout or missing)")
# TODO: probably only want to increment this once per update() call
self.can_invalid_cnt = 0 if valid else min(self.can_invalid_cnt + 1, CAN_INVALID_CNT)
return self.can_invalid_cnt < CAN_INVALID_CNT and counters_valid
def update(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
if self.enable_capture:
self.seen_addresses.add(address)
state = self.message_states.get(address)
if state is None or len(dat) > 64:
continue
if state.parse(t, dat):
updated_addrs.add(address)
vl_addr = self.vl[address]
vl_all_addr = self.vl_all[address]
ts_addr = self.ts_nanos[address]
raw_dat = bytes(dat)
self.dat[address] = raw_dat
self.dat[state.name] = raw_dat
for i, sig in enumerate(state.signals):
vl_addr[sig.name] = state.vals[i]
vl_all_addr[sig.name] = state.all_vals[i]
ts_addr[sig.name] = state.timestamps[-1]
if not bus_empty:
self.last_nonempty_nanos = t
self._last_update_nanos = t
return updated_addrs
class CANDefine:
def __init__(self, dbc_name: str):
dbc = DBC(dbc_name)
dv = defaultdict(dict)
for val in dbc.vals:
sgname = val.name
address = val.address
msg = 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
artifacts/package_runtime/iqdbc/can/tests/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
*.bz2

8
artifacts/package_runtime/iqdbc/can/tests/__init__.py Normal file
View File

@@ -0,0 +1,8 @@
import glob
import os
from iqdbc import DBC_PATH
ALL_DBCS = [os.path.basename(dbc).split('.')[0] for dbc in
glob.glob(f"{DBC_PATH}/*.dbc")]
TEST_DBC = os.path.abspath(os.path.join(os.path.dirname(__file__), "test.dbc"))

46
artifacts/package_runtime/iqdbc/can/tests/benchmark.py Normal file
View File

@@ -0,0 +1,46 @@
#!/usr/bin/env python3
import time
from iqdbc.can import CANPacker, CANParser
def _benchmark(checks, n):
parser = CANParser('toyota_new_mc_pt_generated', checks, 0)
packer = CANPacker('toyota_new_mc_pt_generated')
t1 = time.process_time_ns()
can_msgs = []
for i in range(10000):
values = {"ACC_CONTROL": {"ACC_TYPE": 1, "ALLOW_LONG_PRESS": 3}}
msgs = [packer.make_can_msg(k, 0, v) for k, v in values.items()]
can_msgs.append([int(0.01 * i * 1e9), msgs])
t2 = time.process_time_ns()
pack_dt = t2 - t1
ets = []
for _ in range(25):
if n > 1:
strings = []
for i in range(0, len(can_msgs), n):
strings.append(can_msgs[i:i + n])
t1 = time.process_time_ns()
for m in strings:
parser.update(m)
t2 = time.process_time_ns()
else:
t1 = time.process_time_ns()
for m in can_msgs:
parser.update([m])
t2 = time.process_time_ns()
ets.append(t2 - t1)
et = sum(ets) / len(ets)
avg_nanos = et / len(can_msgs)
print('[%d] %.1fms to pack, %.1fms to parse %s messages, avg: %dns' % (n, pack_dt/1e6, et/1e6, len(can_msgs), avg_nanos))
if __name__ == "__main__":
# python -m cProfile -s cumulative benchmark.py
_benchmark([('ACC_CONTROL', 10)], 1)
_benchmark([('ACC_CONTROL', 10)], 5)
_benchmark([('ACC_CONTROL', 10)], 10)

27
artifacts/package_runtime/iqdbc/can/tests/test.dbc Normal file
View File

@@ -0,0 +1,27 @@
CM_ "This DBC is used for the CAN parser and packer tests.";
BO_ 228 STEERING_CONTROL: 5 EON
SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS
SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS
SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS
SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS
SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS
SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS
SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS
BO_ 316 Brake_Status: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Signal1 : 12|46@1+ (1,0) [0|1] "" XXX
SG_ ES_Brake : 58|1@1+ (1,0) [0|1] "" XXX
SG_ Signal2 : 59|3@1+ (1,0) [0|1] "" XXX
SG_ Brake : 62|1@1+ (1,0) [0|1] "" XXX
SG_ Signal3 : 63|1@1+ (1,0) [0|1] "" XXX
BO_ 245 CAN_FD_MESSAGE: 32 XXX
SG_ COUNTER : 7|8@0+ (1,0) [0|1] "" XXX
SG_ SIGNED : 22|16@0- (1,0) [0|1] "" XXX
SG_ 64_BIT_LE : 159|64@1+ (1,0) [0|1] "" XXX
SG_ 64_BIT_BE : 80|64@0+ (1,0) [0|1] "" XXX
VAL_ 80 NON_EXISTENT_ADDR 0 "test";

594
artifacts/package_runtime/iqdbc/can/tests/test_checksums.py Normal file
View File

@@ -0,0 +1,594 @@
import copy
from iqdbc.can import CANPacker, CANParser
class TestCanChecksums:
def verify_checksum(self, subtests, dbc_file: str, msg_name: str, msg_addr: int, test_messages: list[bytes],
checksum_field: str = 'CHECKSUM', counter_field = 'COUNTER'):
"""
Verify that iqdbc calculates payload CRCs/checksums matching those received in known-good sample messages
Depends on all non-zero bits in the sample message having a corresponding DBC signal, add UNKNOWN signals if needed
"""
parser = CANParser(dbc_file, [(msg_name, 0)], 0)
packer = CANPacker(dbc_file)
for data in test_messages:
expected_msg = (msg_addr, data, 0)
parser.update([0, [expected_msg]])
expected = copy.deepcopy(parser.vl[msg_name])
modified = copy.deepcopy(expected)
modified.pop(checksum_field, None)
modified_msg = packer.make_can_msg(msg_name, 0, modified)
parser.update([0, [modified_msg]])
tested = parser.vl[msg_name]
with subtests.test(counter=expected[counter_field]):
assert tested[checksum_field] == expected[checksum_field]
def verify_fca_giorgio_crc(self, subtests, msg_name: str, msg_addr: int, test_messages: list[bytes]):
"""Test modified SAE J1850 CRCs, with special final XOR cases for EPS messages"""
assert len(test_messages) == 3
self.verify_checksum(subtests, "fca_giorgio", msg_name, msg_addr, test_messages)
def test_fca_giorgio_eps_1(self, subtests):
self.verify_fca_giorgio_crc(subtests, "EPS_1", 0xDE, [
b'\x17\x51\x97\xcc\x00\xdf',
b'\x17\x51\x97\xc9\x01\xa3',
b'\x17\x51\x97\xcc\x02\xe5',
])
def test_fca_giorgio_eps_2(self, subtests):
self.verify_fca_giorgio_crc(subtests, "EPS_2", 0x106, [
b'\x7c\x43\x57\x60\x00\x00\xa1',
b'\x7c\x63\x58\xe0\x00\x01\xd5',
b'\x7c\x63\x58\xe0\x00\x02\xf2',
])
def test_fca_giorgio_eps_3(self, subtests):
self.verify_fca_giorgio_crc(subtests, "EPS_3", 0x122, [
b'\x7b\x30\x00\xf8',
b'\x7b\x10\x01\x90',
b'\x7b\xf0\x02\x6e',
])
def test_fca_giorgio_abs_2(self, subtests):
self.verify_fca_giorgio_crc(subtests, "ABS_2", 0xFE, [
b'\x7e\x38\x00\x7d\x10\x31\x80\x32',
b'\x7e\x38\x00\x7d\x10\x31\x81\x2f',
b'\x7e\x38\x00\x7d\x20\x31\x82\x20',
])
def test_honda_checksum(self):
"""Test checksums for Honda standard and extended CAN ids"""
# TODO: refactor to use self.verify_checksum()
dbc_file = "honda_civic_hatchback_ex_2017_can_generated"
msgs = [("LKAS_HUD", 0), ("LKAS_HUD_A", 0)]
parser = CANParser(dbc_file, msgs, 0)
packer = CANPacker(dbc_file)
values = {
'LKAS_READY': 1,
'LKAS_STATE_CHANGE': 1,
'STEERING_REQUIRED': 1,
'SOLID_LANES': 1,
'BEEP': 0,
}
# known correct checksums according to the above values
checksum_std = [11, 10, 9, 8]
checksum_ext = [4, 3, 2, 1]
for std, ext in zip(checksum_std, checksum_ext, strict=True):
msgs = [
packer.make_can_msg("LKAS_HUD", 0, values),
packer.make_can_msg("LKAS_HUD_A", 0, values),
]
parser.update([0, msgs])
assert parser.vl['LKAS_HUD']['CHECKSUM'] == std
assert parser.vl['LKAS_HUD_A']['CHECKSUM'] == ext
def test_honda_checksum_high_extended(self):
"""Extended CAN ids above 0x100000 use a +10 checksum constant instead of +3"""
dbc_file = "honda_common_canfd_generated"
msgs = [("LANE_PATH", 0), ("RADAR_LEAD", 0)]
parser = CANParser(dbc_file, msgs, 0)
packer = CANPacker(dbc_file)
lane_path_values = {
'MUX': 1,
'PATH_OFFSET_1': 0,
'PATH_OFFSET_2': 0,
'PATH_OFFSET_3': 2047,
'PATH_OFFSET_4': 2047,
}
radar_lead_values = {
'CNTR_REF': 2,
'SET_ME_X01': 1,
'TARGET_SPEED_MAYBE': 140,
'LEFT_LANE': 3,
'RIGHT_LANE': 3,
'LANE_PATH_LENGTH': 6,
}
# known correct checksums according to the above values
checksum_lane_path = [14, 13, 12, 11]
checksum_radar_lead = [4, 3, 2, 1]
for lane_path, radar_lead in zip(checksum_lane_path, checksum_radar_lead, strict=True):
msgs = [
packer.make_can_msg("LANE_PATH", 0, lane_path_values),
packer.make_can_msg("RADAR_LEAD", 0, radar_lead_values),
]
parser.update([0, msgs])
assert parser.vl['LANE_PATH']['CHECKSUM'] == lane_path
assert parser.vl['RADAR_LEAD']['CHECKSUM'] == radar_lead
assert parser.can_valid
def verify_volkswagen_mqb_crc(self, subtests, msg_name: str, msg_addr: int, test_messages: list[bytes], counter_field: str = 'COUNTER'):
"""Test AUTOSAR E2E Profile 2 CRCs"""
assert len(test_messages) == 16 # All counter values must be tested
self.verify_checksum(subtests, "vw_mqb", msg_name, msg_addr, test_messages, counter_field=counter_field)
def test_volkswagen_mqb_crc_lwi_01(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "LWI_01", 0x86, [
b'\x6b\x00\xbd\x00\x00\x00\x00\x00',
b'\xee\x01\x0a\x00\x00\x00\x00\x00',
b'\xd8\x02\xa9\x00\x00\x00\x00\x00',
b'\x03\x03\xbe\xa2\x12\x00\x00\x00',
b'\x7b\x04\x31\x20\x03\x00\x00\x00',
b'\x8b\x05\xe2\x85\x09\x00\x00\x00',
b'\x63\x06\x13\x21\x00\x00\x00\x00',
b'\x66\x07\x05\x00\x00\x00\x00\x00',
b'\x49\x08\x0d\x00\x00\x00\x00\x00',
b'\x5f\x09\x7e\x60\x01\x00\x00\x00',
b'\xaf\x0a\x72\x20\x00\x00\x00\x00',
b'\x59\x0b\x1b\x00\x00\x00\x00\x00',
b'\xa8\x0c\x06\x00\x00\x00\x00\x00',
b'\xbc\x0d\x72\x20\x00\x00\x00\x00',
b'\xf9\x0e\x0f\x00\x00\x00\x00\x00',
b'\x60\x0f\x62\xc0\x00\x00\x00\x00',
])
def test_volkswagen_mqb_crc_airbag_01(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "Airbag_01", 0x40, [
b'\xaf\x00\x00\x80\xc0\x00\x20\x3e',
b'\x54\x01\x00\x80\xc0\x00\x20\x1a',
b'\x54\x02\x00\x80\xc0\x00\x60\x00',
b'\x31\x03\x00\x80\xc0\x00\x60\xf2',
b'\xe0\x04\x00\x80\xc0\x00\x60\xcc',
b'\xb3\x05\x00\x80\xc0\x00\x40\xde',
b'\xa4\x06\x00\x80\xc0\x00\x40\x18',
b'\x94\x07\x00\x80\xc0\x00\x20\x38',
b'\x2d\x08\x00\x80\xc0\x00\x60\xae',
b'\xc2\x09\x00\x80\xc0\x00\x00\x1c',
b'\x1f\x0a\x00\x80\xc0\x00\x60\x2c',
b'\x7f\x0b\x00\x80\xc0\x00\x00\x00',
b'\x03\x0c\x00\x80\xc0\x00\x40\xd6',
b'\x56\x0d\x00\x80\xc0\x00\x20\x50',
b'\x4a\x0e\x00\x80\xc0\x00\x20\xf2',
b'\xe5\x0f\x00\x80\xc0\x00\x40\xf6',
])
def test_volkswagen_mqb_crc_lh_eps_03(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "LH_EPS_03", 0x9F, [
b'\x11\x30\x2e\x00\x05\x1c\x80\x30',
b'\x5b\x31\x8e\x03\x05\x53\x00\x30',
b'\xcb\x32\xd3\x06\x05\x73\x00\x30',
b'\xf2\x33\x28\x00\x05\x26\x00\x30',
b'\x0b\x34\x44\x00\x05\x5b\x80\x30',
b'\xed\x35\x80\x00\x03\x34\x00\x30',
b'\xf0\x36\x88\x00\x05\x3d\x80\x30',
b'\x9e\x37\x44\x03\x05\x41\x00\x30',
b'\x68\x38\x06\x01\x05\x18\x80\x30',
b'\x87\x39\x51\x00\x05\x11\x80\x30',
b'\x8c\x3a\x29\x00\x05\xac\x00\x30',
b'\x08\x3b\xbd\x00\x05\x8e\x00\x30',
b'\xd4\x3c\x19\x00\x05\x05\x80\x30',
b'\x29\x3d\x54\x00\x05\x5b\x00\x30',
b'\xa1\x3e\x49\x01\x03\x04\x80\x30',
b'\xe2\x3f\x05\x00\x05\x0a\x00\x30',
])
def test_volkswagen_mqb_crc_getriebe_11(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "Getriebe_11", 0xAD, [
b'\xf8\xe0\xbf\xff\x5f\x20\x20\x20',
b'\xb0\xe1\xbf\xff\xc6\x98\x21\x80',
b'\xd2\xe2\xbf\xff\x5f\x20\x20\x20',
b'\x00\xe3\xbf\xff\xaa\x20\x20\x10',
b'\xf1\xe4\xbf\xff\x5f\x20\x20\x20',
b'\xc4\xe5\xbf\xff\x5f\x20\x20\x20',
b'\xda\xe6\xbf\xff\x5f\x20\x20\x20',
b'\x85\xe7\xbf\xff\x5f\x20\x20\x20',
b'\x12\xe8\xbf\xff\x5f\x20\x20\x20',
b'\x45\xe9\xbf\xff\xaa\x20\x20\x10',
b'\x03\xea\xbf\xff\xcc\x20\x20\x10',
b'\xfc\xeb\xbf\xff\x5f\x20\x21\x20',
b'\xfe\xec\xbf\xff\xad\x20\x20\x10',
b'\xbd\xed\xbf\xff\xaa\x20\x20\x10',
b'\x67\xee\xbf\xff\xaa\x20\x20\x10',
b'\x36\xef\xbf\xff\xaa\x20\x20\x10',
], counter_field="COUNTER_DISABLED") # see iqdbc#1235
def test_volkswagen_mqb_crc_esp_21(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ESP_21", 0xFD, [
b'\x66\xd0\x1f\x80\x45\x05\x00\x00',
b'\x87\xd1\x1f\x80\x52\x05\x00\x00',
b'\xcd\xd2\x1f\x80\x50\x06\x00\x00',
b'\xfd\xd3\x1f\x80\x35\x02\x00\x00',
b'\xfa\xd4\x1f\x80\x22\x05\x00\x00',
b'\xfd\xd5\x1f\x80\x84\x04\x00\x00',
b'\x2e\xd6\x1f\x80\xf0\x03\x00\x00',
b'\x9f\xd7\x1f\x80\x00\x00\x00\x00',
b'\x1e\xd8\x1f\x80\xb3\x03\x00\x00',
b'\x61\xd9\x1f\x80\x6d\x05\x00\x00',
b'\x44\xda\x1f\x80\x47\x02\x00\x00',
b'\x86\xdb\x1f\x80\x3a\x02\x00\x00',
b'\x39\xdc\x1f\x80\xcb\x01\x00\x00',
b'\x19\xdd\x1f\x80\x00\x00\x00\x00',
b'\x8c\xde\x1f\x80\xba\x04\x00\x00',
b'\xfb\xdf\x1f\x80\x46\x00\x00\x00',
])
def test_volkswagen_mqb_crc_esp_02(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ESP_02", 0x101, [
b'\xf2\x00\x7e\xff\xa1\x2a\x40\x00',
b'\xd3\x01\x7d\x00\xa2\x0c\x02\x00',
b'\x03\x02\x7a\x06\xa2\x49\x42\x00',
b'\xfd\x03\x70\xfb\xa1\xde\x00\x00',
b'\x8e\x04\x7b\xf7\xa1\xd2\x01\x00',
b'\x0f\x05\x7d\xfd\xa1\x31\x40\x00',
b'\xb6\x06\x7d\x01\xa2\x0a\x40\x00',
b'\xe8\x07\x7e\xfd\xa1\x12\x40\x00',
b'\x74\x08\x7a\x01\xa2\x40\x01\x00',
b'\xe3\x09\x81\x00\xa2\xb5\x01\x00',
b'\xab\x0a\x74\x09\xa2\x9f\x42\x00',
b'\xf3\x0b\x80\x12\xa2\x94\x00\x00',
b'\x88\x0c\x7f\x07\xa2\x46\x00\x00',
b'\x6f\x0d\x7f\xff\xa1\x53\x40\x00',
b'\x38\x0e\x73\xd6\xa1\x6a\x40\x00',
b'\x49\x0f\x85\x12\xa2\xf6\x01\x00',
])
def test_volkswagen_mqb_crc_esp_05(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ESP_05", 0x106, [
b'\x90\x80\x64\x00\x00\x00\xe7\x10',
b'\xf4\x81\x64\x00\x00\x00\xe7\x10',
b'\x90\x82\x63\x00\x00\x00\xe8\x10',
b'\xa0\x83\x63\x00\x00\x00\xe6\x10',
b'\xe7\x84\x63\x00\x00\x00\xe8\x10',
b'\x2e\x85\x78\x04\x00\x00\xea\x30',
b'\x7b\x86\x63\x00\x00\x00\xe6\x10',
b'\x71\x87\x79\x04\x00\x00\xd0\x30',
b'\x50\x88\x79\x04\x00\x00\xea\x30',
b'\x81\x89\x64\x00\x00\x00\xe1\x10',
b'\x6a\x8a\x68\x00\x00\x04\xd0\x10',
b'\x17\x8b\x6a\x04\x00\x00\xe6\x10',
b'\xc7\x8c\x63\x00\x00\x00\xd1\x10',
b'\x53\x8d\x64\x04\x00\x00\xe2\x10',
b'\x24\x8e\x63\x00\x00\x00\xe7\x10',
b'\x3f\x8f\x82\x04\x00\x00\xe6\x30',
])
def test_volkswagen_mqb_crc_esp_10(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ESP_10", 0x116, [
b'\x2d\x00\xd5\x98\x9f\x26\x25\x0f',
b'\x24\x01\x60\x63\x2c\x5e\x3b\x0f',
b'\x08\x02\xb2\x2f\xee\x9a\x29\x0f',
b'\x7c\x03\x17\x07\x1d\xe5\x8c\x0f',
b'\xaa\x04\xd6\xe3\xeb\x98\xe8\x0f',
b'\x4e\x05\xbb\xd9\x65\x43\xca\x0f',
b'\x59\x06\x78\xbd\x25\xc6\xf2\xff',
b'\xaf\x07\x42\x85\x53\xbe\xbe\x0f',
b'\x2a\x08\xa6\xcd\x95\x8c\x12\x0f',
b'\xce\x09\x6e\x17\x6d\x1b\x2f\x0f',
b'\x60\x0a\xd3\xe6\x3a\x8d\xf0\x0f',
b'\xc5\x0b\xfc\x69\x57\x50\x21\x0f',
b'\x70\x0c\xde\xf3\x9d\xe9\x6b\xff',
b'\x62\x0d\xc4\x1a\xdb\x61\x7a\x0f',
b'\x76\x0e\x79\x69\xe3\x32\x67\x0f',
b'\x15\x0f\x51\x59\x56\x35\xb1\x0f',
])
def test_volkswagen_mqb_crc_acc_10(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ACC_10", 0x117, [
b'\x9b\x00\x00\x40\x68\x00\x00\xff',
b'\xff\x01\x00\x40\x68\x00\x00\xff',
b'\x53\x02\x00\x40\x68\x00\x00\xff',
b'\x37\x03\x00\x40\x68\x00\x00\xff',
b'\x24\x04\x00\x40\x68\x00\x00\xff',
b'\x40\x05\x00\x40\x68\x00\x00\xff',
b'\xec\x06\x00\x40\x68\x00\x00\xff',
b'\x88\x07\x00\x40\x68\x00\x00\xff',
b'\xca\x08\x00\x40\x68\x00\x00\xff',
b'\xae\x09\x00\x40\x68\x00\x00\xff',
b'\x02\x0a\x00\x40\x68\x00\x00\xff',
b'\x66\x0b\x00\x40\x68\x00\x00\xff',
b'\x75\x0c\x00\x40\x68\x00\x00\xff',
b'\x11\x0d\x00\x40\x68\x00\x00\xff',
b'\xbd\x0e\x00\x40\x68\x00\x00\xff',
b'\xd9\x0f\x00\x40\x68\x00\x00\xff',
])
def test_volkswagen_mqb_crc_tsk_06(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "TSK_06", 0x120, [
b'\xc1\x00\x00\x02\x00\x08\xff\x21',
b'\x34\x01\x00\x02\x00\x08\xff\x21',
b'\xcc\x02\x00\x02\x00\x08\xff\x21',
b'\x1e\x03\x00\x02\x00\x08\xff\x21',
b'\x48\x04\x00\x02\x00\x08\xff\x21',
b'\x4a\x05\x00\x02\x00\x08\xff\x21',
b'\xa5\x06\x00\x02\x00\x08\xff\x21',
b'\xa7\x07\x00\x02\x00\x08\xff\x21',
b'\xfe\x08\x00\x02\x00\x08\xff\x21',
b'\xa8\x09\x00\x02\x00\x08\xff\x21',
b'\x73\x0a\x00\x02\x00\x08\xff\x21',
b'\xdf\x0b\x00\x02\x00\x08\xff\x21',
b'\x05\x0c\x00\x02\x00\x08\xff\x21',
b'\xb5\x0d\x00\x02\x00\x08\xff\x21',
b'\xde\x0e\x00\x02\x00\x08\xff\x21',
b'\x0b\x0f\x00\x02\x00\x08\xff\x21',
])
def test_volkswagen_mqb_crc_motor_20(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "Motor_20", 0x121, [
b'\xb9\x00\x00\xc0\x39\x46\x7e\xfe',
b'\x85\x31\x20\x00\x1a\x46\x7e\xfe',
b'\xc7\x12\x00\x40\x1a\x46\x7e\xfe',
b'\x53\x93\x00\x00\x19\x46\x7e\xfe',
b'\xa4\x34\x00\x80\x1a\x46\x7e\xfe',
b'\x0e\x55\x20\x60\x18\x46\x7e\xfe',
b'\x3f\x06\x00\xc0\x37\x4c\x7e\xfe',
b'\x0c\x07\x00\x40\x39\x46\x7e\xfe',
b'\x2a\x08\x00\x00\x3a\x46\x7e\xfe',
b'\x7f\x49\x20\x80\x1a\x46\x7e\xfe',
b'\x2f\x0a\x00\xc0\x39\x46\x7e\xfe',
b'\x70\xbb\x00\x00\x17\x46\x7e\xfe',
b'\x06\x0c\x00\x00\x39\x46\x7e\xfe',
b'\x4b\x9d\x20\xe0\x16\x4c\x7e\xfe',
b'\x73\xfe\x00\x40\x16\x46\x7e\xfe',
b'\xaf\x0f\x20\x80\x39\x4c\x7e\xfe',
])
def test_volkswagen_mqb_crc_acc_06(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ACC_06", 0x122, [
b'\x14\x80\x00\xfe\x07\x00\x00\x18',
b'\x9f\x81\x00\xfe\x07\x00\x00\x18',
b'\x0a\x82\x00\xfe\x07\x00\x00\x28',
b'\x40\x83\x00\xfe\x07\x00\x00\x18',
b'\x2d\x84\x00\xfe\x07\x00\x00\x28',
b'\xdb\x85\x00\xfe\x07\x00\x00\x18',
b'\x4d\x86\x00\xfe\x07\x00\x00\x28',
b'\x35\x87\x00\xfe\x07\x00\x00\x18',
b'\x23\x88\x00\xfe\x07\x00\x00\x28',
b'\x4a\x89\x00\xfe\x07\x00\x00\x28',
b'\xe1\x8a\x00\xfe\x07\x00\x00\x28',
b'\x30\x8b\x00\xfe\x07\x00\x00\x28',
b'\x60\x8c\x00\xfe\x07\x00\x00\x28',
b'\x0d\x8d\x00\xfe\x07\x00\x00\x18',
b'\x8c\x8e\x00\xfe\x07\x00\x00\x18',
b'\x6f\x8f\x00\xfe\x07\x00\x00\x28',
])
def test_volkswagen_mqb_crc_hca_01(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "HCA_01", 0x126, [
b'\x00\x30\x0d\xc0\x05\xfe\x07\x00',
b'\x3e\x31\x54\xc0\x05\xfe\x07\x00',
b'\xa7\x32\xbb\x40\x05\xfe\x07\x00',
b'\x96\x33\x29\xc0\x05\xfe\x07\x00',
b'\x5f\x34\x00\x00\x03\xfe\x07\x00',
b'\x3b\x35\xae\x40\x05\xfe\x07\x00',
b'\xc7\x36\x7a\x40\x05\xfe\x07\x00',
b'\x6f\x37\x76\x40\x05\xfe\x07\x00',
b'\xb1\x38\x00\x00\x03\xfe\x07\x00',
b'\xd5\x39\x00\x00\x03\xfe\x07\x00',
b'\xba\x3a\x69\xc0\x05\xfe\x07\x00',
b'\x65\x3b\x10\x40\x05\xfe\x07\x00',
b'\x49\x3c\x72\xc0\x05\xfe\x07\x00',
b'\xc6\x3d\xdf\x40\x05\xfe\x07\x00',
b'\x1d\x3e\x2c\xc1\x05\xfe\x07\x00',
b'\x9b\x3f\x20\x40\x05\xfe\x07\x00',
])
def test_volkswagen_mqb_crc_gra_acc_01(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "GRA_ACC_01", 0x12B, [
b'\x86\x40\x80\x2a\x00\x00\x00\x00',
b'\xf4\x41\x80\x2a\x00\x00\x00\x00',
b'\x50\x42\x80\x2a\x00\x00\x00\x00',
b'\x08\x43\x80\x2a\x00\x00\x00\x00',
b'\x88\x44\x80\x2a\x00\x00\x00\x00',
b'\x2d\x45\x80\x2a\x00\x00\x00\x00',
b'\x34\x46\x80\x2a\x00\x00\x00\x00',
b'\x11\x47\x80\x2a\x00\x00\x00\x00',
b'\xc4\x48\x80\x2a\x00\x00\x00\x00',
b'\xcc\x49\x80\x2a\x00\x00\x00\x00',
b'\xdc\x4a\x80\x2a\x00\x00\x00\x00',
b'\x79\x4b\x80\x2a\x00\x00\x00\x00',
b'\x3c\x4c\x80\x2a\x00\x00\x00\x00',
b'\x68\x4d\x80\x2a\x00\x00\x00\x00',
b'\x27\x4e\x80\x2a\x00\x00\x00\x00',
b'\x0d\x4f\x80\x2a\x00\x00\x00\x00',
])
def test_volkswagen_mqb_crc_acc_07(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ACC_07", 0x12E, [
b'\xac\xe0\x7f\x00\xfe\x00\xc0\xff',
b'\xa2\xe1\x7f\x00\xfe\x00\xc0\xff',
b'\x6b\xe2\x7f\x00\xfe\x00\xc0\xff',
b'\xf2\xe3\x7f\x00\xfe\x00\xc0\xff',
b'\xd5\xe4\x7f\x00\xfe\x00\xc0\xff',
b'\x35\xe5\x7f\x00\xfe\x00\xc0\xff',
b'\x7f\xe6\x7f\x00\xfe\x00\xc0\xff',
b'\x6c\xe7\x7f\x00\xfe\x00\xc0\xff',
b'\x05\xe8\x7f\x00\xfe\x00\xc0\xff',
b'\x79\xe9\x7f\x00\xfe\x00\xc0\xff',
b'\x25\xea\x7f\x00\xfe\x00\xc0\xff',
b'\xd1\xeb\x7f\x00\xfe\x00\xc0\xff',
b'\x72\xec\x7f\x00\xfe\x00\xc0\xff',
b'\x58\xed\x7f\x00\xfe\x00\xc0\xff',
b'\x82\xee\x7f\x00\xfe\x00\xc0\xff',
b'\x85\xef\x7f\x00\xfe\x00\xc0\xff',
])
def test_volkswagen_mqb_crc_motor_ev_01(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "Motor_EV_01", 0x187, [
b'\x70\x80\x15\x00\x00\x00\x00\xF0',
b'\x07\x81\x15\x00\x00\x00\x00\xF0',
b'\x7A\x82\x15\x00\x00\x00\x00\xF0',
b'\x26\x83\x15\x00\x00\x00\x00\xF0',
b'\xBE\x84\x15\x00\x00\x00\x00\xF0',
b'\x5A\x85\x15\x00\x00\x00\x00\xF0',
b'\xFC\x86\x15\x00\x00\x00\x00\xF0',
b'\x9E\x87\x15\x00\x00\x00\x00\xF0',
b'\xAF\x88\x15\x00\x00\x00\x00\xF0',
b'\x35\x89\x15\x00\x00\x00\x00\xF0',
b'\xC5\x8A\x15\x00\x00\x00\x00\xF0',
b'\x11\x8B\x15\x00\x00\x00\x00\xF0',
b'\xD0\x8C\x15\x00\x00\x00\x00\xF0',
b'\xE8\x8D\x15\x00\x00\x00\x00\xF0',
b'\xF5\x8E\x15\x00\x00\x00\x00\xF0',
b'\x00\x8F\x15\x00\x00\x00\x00\xF0',
])
def test_volkswagen_mqb_crc_esp_33(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ESP_33", 0x1AB, [
b'\x64\x00\x80\x02\x00\x00\x00\x00',
b'\x19\x01\x00\x00\x00\x00\x00\x00',
b'\xfc\x02\x00\x10\x01\x00\x00\x00',
b'\x8b\x03\x80\x02\x00\x00\x00\x00',
b'\xa4\x04\x00\x10\x01\x00\x00\x00',
b'\x97\x05\x00\x02\x00\x00\x01\x00',
b'\xd5\x06\x80\x02\x00\x00\x01\x00',
b'\xa0\x07\x80\x02\x00\x00\x01\x00',
b'\x89\x08\x00\x00\x00\x00\x00\x00',
b'\xe3\x09\x00\x00\x00\x00\x00\x00',
b'\x0e\x0a\x00\x00\x00\x00\x00\x00',
b'\x90\x0b\x00\x00\x00\x00\x00\x00',
b'\x32\x0c\x00\x10\x01\x00\x00\x00',
b'\x30\x0d\x00\x00\x00\x00\x00\x00',
b'\xc2\x0e\x00\x10\x01\x00\x00\x00',
b'\x68\x0f\x80\x02\x00\x00\x00\x00',
])
def test_volkswagen_mqb_crc_acc_02(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ACC_02", 0x30C, [
b'\x82\xf0\x3f\x00\x40\x30\x00\x40',
b'\xe6\xf1\x3f\x00\x40\x30\x00\x40',
b'\x4a\xf2\x3f\x00\x40\x30\x00\x40',
b'\x2e\xf3\x3f\x00\x40\x30\x00\x40',
b'\x3d\xf4\x3f\x00\x40\x30\x00\x40',
b'\x59\xf5\x3f\x00\x40\x30\x00\x40',
b'\xf5\xf6\x3f\x00\x40\x30\x00\x40',
b'\x91\xf7\x3f\x00\x40\x30\x00\x40',
b'\xd3\xf8\x3f\x00\x40\x30\x00\x40',
b'\xb7\xf9\x3f\x00\x40\x30\x00\x40',
b'\x1b\xfa\x3f\x00\x40\x30\x00\x40',
b'\x7f\xfb\x3f\x00\x40\x30\x00\x40',
b'\x6c\xfc\x3f\x00\x40\x30\x00\x40',
b'\x08\xfd\x3f\x00\x40\x30\x00\x40',
b'\xa4\xfe\x3f\x00\x40\x30\x00\x40',
b'\xc0\xff\x3f\x00\x40\x30\x00\x40',
])
def test_volkswagen_mqb_crc_swa_01(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "SWA_01", 0x30F, [
b'\x10\x00\x10\x00\x00\x00\x00\x00',
b'\x74\x01\x10\x00\x00\x00\x00\x00',
b'\xD8\x02\x10\x00\x00\x00\x00\x00',
b'\xBC\x03\x10\x00\x00\x00\x00\x00',
b'\xAF\x04\x10\x00\x00\x00\x00\x00',
b'\xCB\x05\x10\x00\x00\x00\x00\x00',
b'\x67\x06\x10\x00\x00\x00\x00\x00',
b'\x03\x07\x10\x00\x00\x00\x00\x00',
b'\x41\x08\x10\x00\x00\x00\x00\x00',
b'\x25\x09\x10\x00\x00\x00\x00\x00',
b'\x89\x0A\x10\x00\x00\x00\x00\x00',
b'\xED\x0B\x10\x00\x00\x00\x00\x00',
b'\xFE\x0C\x10\x00\x00\x00\x00\x00',
b'\x9A\x0D\x10\x00\x00\x00\x00\x00',
b'\x36\x0E\x10\x00\x00\x00\x00\x00',
b'\x52\x0F\x10\x00\x00\x00\x00\x00',
])
def test_volkswagen_mqb_crc_acc_04(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ACC_04", 0x324, [
b'\xba\x00\x00\x00\x00\x00\x00\x10',
b'\xde\x01\x00\x00\x00\x00\x00\x10',
b'\x72\x02\x00\x00\x00\x00\x00\x10',
b'\x16\x03\x00\x00\x00\x00\x00\x10',
b'\x05\x04\x00\x00\x00\x00\x00\x10',
b'\x44\x05\x00\x00\x00\x00\x00\x00',
b'\xe8\x06\x00\x00\x00\x00\x00\x00',
b'\xa9\x07\x00\x00\x00\x00\x00\x10',
b'\xeb\x08\x00\x00\x00\x00\x00\x10',
b'\x8f\x09\x00\x00\x00\x00\x00\x10',
b'\x06\x0a\x00\x00\x00\x00\x00\x00',
b'\x47\x0b\x00\x00\x00\x00\x00\x10',
b'\x71\x0c\x00\x00\x00\x00\x00\x00',
b'\x15\x0d\x00\x00\x00\x00\x00\x00',
b'\xb9\x0e\x00\x00\x00\x00\x00\x00',
b'\xdd\x0f\x00\x00\x00\x00\x00\x00',
])
def test_volkswagen_mqb_crc_klemmen_status_01(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "Klemmen_Status_01", 0x3C0, [
b'\x74\x00\x03\x00',
b'\xc1\x01\x03\x00',
b'\x31\x02\x03\x00',
b'\x84\x03\x03\x00',
b'\xfe\x04\x03\x00',
b'\x4b\x05\x03\x00',
b'\xbb\x06\x03\x00',
b'\x0e\x07\x03\x00',
b'\x4f\x08\x03\x00',
b'\xfa\x09\x03\x00',
b'\x0a\x0a\x03\x00',
b'\xbf\x0b\x03\x00',
b'\xc5\x0c\x03\x00',
b'\x70\x0d\x03\x00',
b'\x80\x0e\x03\x00',
b'\x35\x0f\x03\x00',
])
def test_volkswagen_mqb_crc_licht_anf_01(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "Licht_Anf_01", 0x3D5, [
b'\xc8\x00\x00\x04\x00\x00\x00\x00',
b'\x9f\x01\x00\x04\x00\x00\x00\x00',
b'\x5e\x02\x00\x04\x00\x00\x00\x00',
b'\x52\x03\x00\x04\x00\x00\x00\x00',
b'\xf2\x04\x00\x04\x00\x00\x00\x00',
b'\x79\x05\x00\x04\x00\x00\x00\x00',
b'\xe6\x06\x00\x04\x00\x00\x00\x00',
b'\xfd\x07\x00\x04\x00\x00\x00\x00',
b'\xf8\x08\x00\x04\x00\x00\x00\x00',
b'\xc6\x09\x00\x04\x00\x00\x00\x00',
b'\xf5\x0a\x00\x04\x00\x00\x00\x00',
b'\x1a\x0b\x00\x04\x00\x00\x00\x00',
b'\x65\x0c\x00\x04\x00\x00\x00\x00',
b'\x41\x0d\x00\x04\x00\x00\x00\x00',
b'\x7f\x0e\x00\x04\x00\x00\x00\x00',
b'\x98\x0f\x00\x04\x00\x00\x00\x00',
])
def test_volkswagen_mqb_crc_esp_20(self, subtests):
self.verify_volkswagen_mqb_crc(subtests, "ESP_20", 0x65D, [
b'\x98\x30\x2b\x10\x00\x00\x22\x81',
b'\xc8\x31\x2b\x10\x00\x00\x22\x81',
b'\x9d\x32\x2b\x10\x00\x00\x22\x81',
b'\x1f\x33\x2b\x10\x00\x00\x22\x81',
b'\x6e\x34\x2b\x10\x00\x00\x22\x81',
b'\x61\x35\x2b\x10\x00\x00\x22\x81',
b'\x6f\x36\x2b\x10\x00\x00\x22\x81',
b'\xe5\x37\x2b\x10\x00\x00\x22\x81',
b'\xf8\x38\x2b\x10\x00\x00\x22\x81',
b'\xe1\x39\x2b\x10\x00\x00\x22\x81',
b'\xaa\x3a\x2b\x10\x00\x00\x22\x81',
b'\xe6\x3b\x2b\x10\x00\x00\x22\x81',
b'\xef\x3c\x2b\x10\x00\x00\x22\x81',
b'\xbb\x3d\x2b\x10\x00\x00\x22\x81',
b'\x9b\x3e\x2b\x10\x00\x00\x22\x81',
b'\x72\x3f\x2b\x10\x00\x00\x22\x81',
])

29
artifacts/package_runtime/iqdbc/can/tests/test_dbc_exceptions.py Normal file
View File

@@ -0,0 +1,29 @@
import pytest
from iqdbc.can import CANDefine, CANPacker, CANParser
from iqdbc.can.tests import TEST_DBC
class TestCanParserPackerExceptions:
def test_civic_exceptions(self):
dbc_file = "honda_civic_touring_2016_can_generated"
dbc_invalid = dbc_file + "abcdef"
msgs = [("STEERING_CONTROL", 50)]
with pytest.raises(FileNotFoundError):
CANParser(dbc_invalid, msgs, 0)
with pytest.raises(FileNotFoundError):
CANPacker(dbc_invalid)
with pytest.raises(FileNotFoundError):
CANDefine(dbc_invalid)
with pytest.raises(KeyError):
CANDefine(TEST_DBC)
parser = CANParser(dbc_file, msgs, 0)
with pytest.raises(IndexError):
parser.update([b''])
# Everything is supposed to work below
CANParser(dbc_file, msgs, 0)
CANParser(dbc_file, [], 0)
CANPacker(dbc_file)
CANDefine(dbc_file)

21
artifacts/package_runtime/iqdbc/can/tests/test_dbc_parser.py Normal file
View File

@@ -0,0 +1,21 @@
from iqdbc.can import CANParser
from iqdbc.can.tests import ALL_DBCS
class TestDBCParser:
def test_enough_dbcs(self):
# sanity check that we're running on the real DBCs
assert len(ALL_DBCS) > 20
def test_parse_all_dbcs(self, subtests):
"""
Dynamic DBC parser checks:
- Checksum and counter length, start bit, endianness
- Duplicate message addresses and names
- Signal out of bounds
- All BO_, SG_, VAL_ lines for syntax errors
"""
for dbc in ALL_DBCS:
with subtests.test(dbc=dbc):
CANParser(dbc, [], 0)

27
artifacts/package_runtime/iqdbc/can/tests/test_define.py Normal file
View File

@@ -0,0 +1,27 @@
from iqdbc.can import CANDefine
from iqdbc.can.tests import ALL_DBCS
class TestCANDefine:
def test_civic(self):
dbc_file = "honda_civic_touring_2016_can_generated"
defs = CANDefine(dbc_file)
assert defs.dv[399] == defs.dv['STEER_STATUS']
assert defs.dv[399] == {'STEER_STATUS':
{7: 'PERMANENT_FAULT',
6: 'TMP_FAULT',
5: 'FAULT_1',
4: 'NO_TORQUE_ALERT_2',
3: 'LOW_SPEED_LOCKOUT',
2: 'NO_TORQUE_ALERT_1',
1: 'DRIVER_STEERING',
0: 'NORMAL'}
}
def test_all_dbcs(self, subtests):
# Asserts no exceptions on all DBCs
for dbc in ALL_DBCS:
with subtests.test(dbc=dbc):
CANDefine(dbc)

407
artifacts/package_runtime/iqdbc/can/tests/test_packer_parser.py Normal file
View File

@@ -0,0 +1,407 @@
import pytest
import random
from iqdbc.can import CANPacker, CANParser
from iqdbc.can.tests import TEST_DBC
MAX_BAD_COUNTER = 5
class TestCanParserPacker:
def test_seen_addresses(self):
parser = CANParser(TEST_DBC, [], 0)
parser.update([0, [(0x123, b'\x00', 0), (0x124, b'\x00', 1)]])
assert parser.seen_addresses == {0x123}
parser.enable_capture = False
parser.update([1, [(0x125, b'\x00', 0)]])
assert parser.seen_addresses == {0x123}
def test_packer(self):
packer = CANPacker(TEST_DBC)
for b in range(6):
for i in range(256):
values = {"COUNTER": i}
addr, dat, bus = packer.make_can_msg("CAN_FD_MESSAGE", b, values)
assert addr == 245
assert bus == b
assert dat[0] == i
def test_packer_counter(self):
msgs = [("CAN_FD_MESSAGE", 0), ]
packer = CANPacker(TEST_DBC)
parser = CANParser(TEST_DBC, msgs, 0)
# packer should increment the counter
for i in range(1000):
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {})
parser.update([0, [msg]])
assert parser.vl["CAN_FD_MESSAGE"]["COUNTER"] == (i % 256)
# setting COUNTER should override
for _ in range(100):
cnt = random.randint(0, 255)
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {
"COUNTER": cnt,
"SIGNED": 0
})
parser.update([0, [msg]])
assert parser.vl["CAN_FD_MESSAGE"]["COUNTER"] == cnt
cnt = random.randint(0, 255)
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {"SIGNED": 0}, rx_counter=cnt)
parser.update([0, [msg]])
assert parser.vl["CAN_FD_MESSAGE"]["COUNTER"] == cnt
# then, should resume counting from the override value
cnt = parser.vl["CAN_FD_MESSAGE"]["COUNTER"]
for i in range(100):
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {})
parser.update([0, [msg]])
assert parser.vl["CAN_FD_MESSAGE"]["COUNTER"] == ((cnt + i) % 256)
def test_parser_can_valid(self):
msgs = [("CAN_FD_MESSAGE", 10), ]
packer = CANPacker(TEST_DBC)
parser = CANParser(TEST_DBC, msgs, 0)
# shouldn't be valid initially
assert not parser.can_valid
# not valid until the message is seen
for _ in range(100):
parser.update([0, []])
assert not parser.can_valid
# valid once seen
for i in range(1, 100):
t = int(0.01 * i * 1e9)
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {})
parser.update([t, [msg]])
assert parser.can_valid
def test_lazy_add_not_ignore_alive(self):
"""
Accessing an undeclared message via parser.vl[...] lazily adds it via
_add_message(key) with the default freq=None, which is NOT the same as
declaring it with math.nan (ignore_alive=True). It's treated as "assume
~1Hz, must be seen within ~10s" — so if that message is never fed, the
parser is permanently invalid. Declaring an optional/rarely-sent message
with math.nan (or gating the .vl[...] read entirely) is required to avoid
this; see iqdbc/car/volkswagen/carstate.py's Diagnose_1/EPB_1 bugs.
"""
parser = CANParser(TEST_DBC, [], 0)
assert parser.can_valid
# lazily add STEERING_CONTROL by reading it, without ever declaring it
# or feeding any CAN data for it
_ = parser.vl["STEERING_CONTROL"]
state = parser.message_states[parser.dbc.name_to_msg["STEERING_CONTROL"].address]
assert not state.ignore_alive
# never becomes valid again, no matter how many times it's checked
# (can_valid debounces over MAX_BAD_COUNTER reads before flipping false)
for _ in range(MAX_BAD_COUNTER):
_ = parser.can_valid
for _ in range(20):
assert not parser.can_valid
def test_parser_updated_list(self):
msgs = [("CAN_FD_MESSAGE", 10), ]
parser = CANParser(TEST_DBC, msgs, 0)
packer = CANPacker(TEST_DBC)
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {})
ret = parser.update([0, [msg]])
assert ret == {245}
ret = parser.update([])
assert len(ret) == 0
def test_parser_counter_can_valid(self):
"""
Tests number of allowed bad counters + ensures CAN stays invalid
while receiving invalid messages + that we can recover
"""
msgs = [
("STEERING_CONTROL", 0),
]
packer = CANPacker("honda_civic_touring_2016_can_generated")
parser = CANParser("honda_civic_touring_2016_can_generated", msgs, 0)
msg = packer.make_can_msg("STEERING_CONTROL", 0, {"COUNTER": 0})
# bad static counter, invalid once it's seen MAX_BAD_COUNTER messages
for idx in range(0x1000):
parser.update([0, [msg]])
assert ((idx + 1) < MAX_BAD_COUNTER) == parser.can_valid
# one to recover
msg = packer.make_can_msg("STEERING_CONTROL", 0, {"COUNTER": 1})
parser.update([0, [msg]])
assert parser.can_valid
def test_parser_no_partial_update(self):
"""
Ensure that the CANParser doesn't partially update messages with invalid signals (COUNTER/CHECKSUM).
Previously, the signal update loop would only break once it got to one of these invalid signals,
after already updating most/all of the signals.
"""
msgs = [
("STEERING_CONTROL", 0),
]
packer = CANPacker("honda_civic_touring_2016_can_generated")
parser = CANParser("honda_civic_touring_2016_can_generated", msgs, 0)
def rx_steering_msg(values, bad_checksum=False):
msg = packer.make_can_msg("STEERING_CONTROL", 0, values)
if bad_checksum:
# add 1 to checksum
dat = bytearray(msg[1])
dat[4] = (dat[4] & 0xF0) | ((dat[4] & 0x0F) + 1)
msg = (msg[0], bytes(dat), msg[2])
parser.update([0, [msg]])
rx_steering_msg({"STEER_TORQUE": 100}, bad_checksum=False)
assert parser.vl["STEERING_CONTROL"]["STEER_TORQUE"] == 100
assert parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"] == [100]
for _ in range(5):
rx_steering_msg({"STEER_TORQUE": 200}, bad_checksum=True)
assert parser.vl["STEERING_CONTROL"]["STEER_TORQUE"] == 100
assert parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"] == []
# Even if CANParser doesn't update instantaneous vl, make sure it didn't add invalid values to vl_all
rx_steering_msg({"STEER_TORQUE": 300}, bad_checksum=False)
assert parser.vl["STEERING_CONTROL"]["STEER_TORQUE"] == 300
assert parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"] == [300]
def test_packer_parser(self):
msgs = [
("Brake_Status", 0),
("CAN_FD_MESSAGE", 0),
("STEERING_CONTROL", 0),
]
packer = CANPacker(TEST_DBC)
parser = CANParser(TEST_DBC, msgs, 0)
for steer in range(-256, 255):
for active in (1, 0):
values = {
"STEERING_CONTROL": {
"STEER_TORQUE": steer,
"STEER_TORQUE_REQUEST": active,
},
"Brake_Status": {
"Signal1": 61042322657536.0,
},
"CAN_FD_MESSAGE": {
"SIGNED": steer,
"64_BIT_LE": random.randint(0, 100),
"64_BIT_BE": random.randint(0, 100),
},
}
msgs = [packer.make_can_msg(k, 0, v) for k, v in values.items()]
parser.update([0, msgs])
for k, v in values.items():
for key, val in v.items():
assert parser.vl[k][key] == pytest.approx(val)
# also check address
for sig in ("STEER_TORQUE", "STEER_TORQUE_REQUEST", "COUNTER", "CHECKSUM"):
assert parser.vl["STEERING_CONTROL"][sig] == parser.vl[228][sig]
def test_scale_offset(self):
"""Test that both scale and offset are correctly preserved"""
dbc_file = "honda_civic_touring_2016_can_generated"
msgs = [("VSA_STATUS", 50)]
parser = CANParser(dbc_file, msgs, 0)
packer = CANPacker(dbc_file)
for brake in range(100):
values = {"USER_BRAKE": brake}
msgs = packer.make_can_msg("VSA_STATUS", 0, values)
parser.update([0, [msgs]])
assert parser.vl["VSA_STATUS"]["USER_BRAKE"] == pytest.approx(brake)
def test_subaru(self):
# Subaru is little endian
dbc_file = "subaru_global_2017_generated"
msgs = [("ES_LKAS", 50)]
parser = CANParser(dbc_file, msgs, 0)
packer = CANPacker(dbc_file)
idx = 0
for steer in range(-256, 255):
for active in [1, 0]:
values = {
"LKAS_Output": steer,
"LKAS_Request": active,
"SET_1": 1
}
msgs = packer.make_can_msg("ES_LKAS", 0, values)
parser.update([0, [msgs]])
assert parser.vl["ES_LKAS"]["LKAS_Output"] == pytest.approx(steer)
assert parser.vl["ES_LKAS"]["LKAS_Request"] == pytest.approx(active)
assert parser.vl["ES_LKAS"]["SET_1"] == pytest.approx(1)
assert parser.vl["ES_LKAS"]["COUNTER"] == pytest.approx(idx % 16)
idx += 1
def test_bus_timeout(self):
"""Test CAN bus timeout detection"""
dbc_file = "honda_civic_touring_2016_can_generated"
freq = 100
msgs = [("VSA_STATUS", freq), ("STEER_MOTOR_TORQUE", freq/2)]
parser = CANParser(dbc_file, msgs, 0)
packer = CANPacker(dbc_file)
i = 0
def send_msg(blank=False):
nonlocal i
i += 1
t = i*((1 / freq) * 1e9)
if blank:
msgs = []
else:
msgs = [packer.make_can_msg("VSA_STATUS", 0, {}), ]
parser.update([t, msgs])
# all good, no timeout
for _ in range(1000):
send_msg()
assert not parser.bus_timeout, str(_)
# timeout after 10 blank msgs
for n in range(200):
send_msg(blank=True)
assert (n >= 10) == parser.bus_timeout
# no timeout immediately after seen again
send_msg()
assert not parser.bus_timeout
def test_updated(self):
"""Test updated value dict"""
dbc_file = "honda_civic_touring_2016_can_generated"
msgs = [("VSA_STATUS", 50)]
parser = CANParser(dbc_file, msgs, 0)
packer = CANPacker(dbc_file)
# Make sure nothing is updated
assert len(parser.vl_all["VSA_STATUS"]["USER_BRAKE"]) == 0
idx = 0
for _ in range(10):
# Ensure CANParser holds the values of any duplicate messages over multiple frames
user_brake_vals = [random.randrange(100) for _ in range(random.randrange(5, 10))]
half_idx = len(user_brake_vals) // 2
can_msgs = [[], []]
for frame, brake_vals in enumerate((user_brake_vals[:half_idx], user_brake_vals[half_idx:])):
for user_brake in brake_vals:
values = {"USER_BRAKE": user_brake}
can_msgs[frame].append(packer.make_can_msg("VSA_STATUS", 0, values))
idx += 1
parser.update([[0, m] for m in can_msgs])
vl_all = parser.vl_all["VSA_STATUS"]["USER_BRAKE"]
assert vl_all == user_brake_vals
if len(user_brake_vals):
assert vl_all[-1] == parser.vl["VSA_STATUS"]["USER_BRAKE"]
def test_timestamp_nanos(self):
"""Test message timestamp dict"""
dbc_file = "honda_civic_touring_2016_can_generated"
msgs = [
("VSA_STATUS", 50),
("POWERTRAIN_DATA", 100),
]
parser = CANParser(dbc_file, msgs, 0)
packer = CANPacker(dbc_file)
# Check the default timestamp is zero
for msg in ("VSA_STATUS", "POWERTRAIN_DATA"):
ts_nanos = parser.ts_nanos[msg].values()
assert set(ts_nanos) == {0}
# Check:
# - timestamp is only updated for correct messages
# - timestamp is correct for multiple runs
# - timestamp is from the latest message if updating multiple strings
for _ in range(10):
can_strings = []
log_mono_time = 0
for i in range(10):
log_mono_time = int(0.01 * i * 1e+9)
can_msg = packer.make_can_msg("VSA_STATUS", 0, {})
can_strings.append((log_mono_time, [can_msg]))
parser.update(can_strings)
ts_nanos = parser.ts_nanos["VSA_STATUS"].values()
assert set(ts_nanos) == {log_mono_time}
ts_nanos = parser.ts_nanos["POWERTRAIN_DATA"].values()
assert set(ts_nanos) == {0}
def test_nonexistent_messages(self):
# Ensure we don't allow messages not in the DBC
existing_messages = ("STEERING_CONTROL", 228, "CAN_FD_MESSAGE", 245)
for msg in existing_messages:
CANParser(TEST_DBC, [(msg, 0)], 0)
with pytest.raises(RuntimeError):
new_msg = msg + "1" if isinstance(msg, str) else msg + 1
CANParser(TEST_DBC, [(new_msg, 0)], 0)
def test_track_all_signals(self):
parser = CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 0)], 0)
assert parser.vl["ACC_CONTROL"] == {
"ACCEL_CMD": 0,
"ALLOW_LONG_PRESS": 0,
"ACC_MALFUNCTION": 0,
"RADAR_DIRTY": 0,
"DISTANCE": 0,
"MINI_CAR": 0,
"ACC_TYPE": 0,
"CANCEL_REQ": 0,
"ACC_CUT_IN": 0,
"LEAD_VEHICLE_STOPPED": 0,
"PERMIT_BRAKING": 0,
"RELEASE_STANDSTILL": 0,
"ITS_CONNECT_LEAD": 0,
"ACCEL_CMD_ALT": 0,
"CHECKSUM": 0,
}
def test_disallow_duplicate_messages(self):
CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 5)], 0)
with pytest.raises(RuntimeError):
CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 5), ("ACC_CONTROL", 10)], 0)
with pytest.raises(RuntimeError):
CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 10), ("ACC_CONTROL", 10)], 0)
def test_allow_undefined_msgs(self):
# TODO: we should throw an exception for these, but we need good
# discovery tests in openpilot first
packer = CANPacker("toyota_nodsu_pt_generated")
assert packer.make_can_msg("ACC_CONTROL", 0, {"UNKNOWN_SIGNAL": 0}) == (835, b'\x00\x00\x00\x00\x00\x00\x00N', 0)
assert packer.make_can_msg("UNKNOWN_MESSAGE", 0, {"UNKNOWN_SIGNAL": 0}) == (0, b'', 0)
assert packer.make_can_msg(0, 0, {"UNKNOWN_SIGNAL": 0}) == (0, b'', 0)

252
artifacts/package_runtime/iqdbc/car/__init__.py Normal file
View File

@@ -0,0 +1,252 @@
# functions common among cars
import numpy as np
from dataclasses import dataclass, field
from enum import IntFlag, ReprEnum, StrEnum, EnumType, auto
from dataclasses import replace
from iqdbc.car import structs, uds
from iqdbc.car.can_definitions import CanData
from iqdbc.car.docs_definitions import CarDocs, ExtraCarDocs
DT_CTRL = 0.01 # car state and control loop timestep (s)
# kg of standard extra cargo to count for drive, gas, etc...
STD_CARGO_KG = 136.
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
ButtonType = structs.CarState.ButtonEvent.Type
def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
if val > val_steady + hyst_gap:
val_steady = val - hyst_gap
elif val < val_steady - hyst_gap:
val_steady = val + hyst_gap
return val_steady
def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: dict[int, structs.CarState.ButtonEvent.Type],
unpressed_btn: int = 0) -> list[structs.CarState.ButtonEvent]:
events: list[structs.CarState.ButtonEvent] = []
if cur_btn == prev_btn:
return events
# Add events for button presses, multiple when a button switches without going to unpressed
for pressed, btn in ((False, prev_btn), (True, cur_btn)):
if btn != unpressed_btn:
events.append(structs.CarState.ButtonEvent(pressed=pressed,
type=buttons_dict.get(btn, ButtonType.unknown)))
return events
def gen_empty_fingerprint():
return {i: {} for i in range(8)}
# these params were derived for the Civic and used to calculate params for other cars
class VehicleDynamicsParams:
MASS = 1326. + STD_CARGO_KG
WHEELBASE = 2.70
CENTER_TO_FRONT = WHEELBASE * 0.4
CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT
ROTATIONAL_INERTIA = 2500
TIRE_STIFFNESS_FRONT = 192150
TIRE_STIFFNESS_REAR = 202500
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
def scale_rot_inertia(mass, wheelbase):
return VehicleDynamicsParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (VehicleDynamicsParams.MASS * VehicleDynamicsParams.WHEELBASE ** 2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor):
center_to_rear = wheelbase - center_to_front
tire_stiffness_front = (VehicleDynamicsParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
(center_to_rear / wheelbase) / (VehicleDynamicsParams.CENTER_TO_REAR / VehicleDynamicsParams.WHEELBASE)
tire_stiffness_rear = (VehicleDynamicsParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
(center_to_front / wheelbase) / (VehicleDynamicsParams.CENTER_TO_FRONT / VehicleDynamicsParams.WHEELBASE)
return tire_stiffness_front, tire_stiffness_rear
DbcDict = dict[StrEnum, str]
class Bus(StrEnum):
pt = auto()
aux = auto()
cam = auto()
radar = auto()
adas = auto()
alt = auto()
body = auto()
chassis = auto()
loopback = auto()
main = auto()
party = auto()
ap_party = auto()
def rate_limit(new_value, last_value, dw_step, up_step):
return float(np.clip(new_value, last_value + dw_step, last_value + up_step))
def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False):
dat = [0x02, uds.SERVICE_TYPE.TESTER_PRESENT]
if subaddr is not None:
dat.insert(0, subaddr)
dat.append(0x80 if suppress_response else 0x0) # sub-function
dat.extend([0x0] * (8 - len(dat)))
return CanData(addr, bytes(dat), bus)
def get_safety_config(safety_model: structs.CarParams.SafetyModel, safety_param: int | None = None) -> structs.CarParams.SafetyConfig:
ret = structs.CarParams.SafetyConfig()
ret.safetyModel = safety_model
if safety_param is not None:
ret.safetyParam = safety_param
return ret
class CanBusBase:
offset: int
def __init__(self, CP, fingerprint: dict[int, dict[int, int]] | None) -> None:
if CP is None:
assert fingerprint is not None
num = max([k for k, v in fingerprint.items() if len(v)], default=0) // 4 + 1
else:
num = len(CP.safetyConfigs)
self.offset = 4 * (num - 1)
class CanSignalRateCalculator:
"""
Calculates the instantaneous rate of a CAN signal by using the counter
variable and the known frequency of the CAN message that contains it.
"""
def __init__(self, frequency: int):
self.frequency = frequency
self.previous_value = 0
self.rate = 0
def update(self, current_value: float, updated: bool):
if updated:
self.rate = (current_value - self.previous_value) * self.frequency
self.previous_value = current_value
return self.rate
@dataclass(frozen=True, kw_only=True)
class CarSpecs:
mass: float # kg, curb weight
wheelbase: float # meters
steerRatio: float
centerToFrontRatio: float = 0.5
minSteerSpeed: float = 0.0 # m/s
minEnableSpeed: float = -1.0 # m/s
tireStiffnessFactor: float = 1.0
def override(self, **kwargs):
return replace(self, **kwargs)
class Freezable:
_frozen: bool = False
def freeze(self):
if not self._frozen:
self._frozen = True
def __setattr__(self, *args, **kwargs):
if self._frozen:
raise Exception("cannot modify frozen object")
super().__setattr__(*args, **kwargs)
@dataclass(order=True)
class PlatformConfigBase(Freezable):
car_docs: list[CarDocs] | list[ExtraCarDocs]
specs: CarSpecs
dbc_dict: DbcDict
flags: int = 0
iq_flags: int = 0
platform_str: str | None = None
origin_car_docs: list[CarDocs] | list[ExtraCarDocs] = field(init=False)
def __hash__(self) -> int:
return hash(self.platform_str)
def override(self, **kwargs):
return replace(self, **kwargs)
def init(self):
pass
def __post_init__(self):
self.origin_car_docs = self.car_docs
self.init()
def get_all_docs(self):
return self.origin_car_docs
@dataclass(order=True)
class PlatformConfig(PlatformConfigBase):
car_docs: list[CarDocs]
specs: CarSpecs
dbc_dict: DbcDict
@dataclass(order=True)
class ExtraPlatformConfig(PlatformConfigBase):
car_docs: list[ExtraCarDocs]
specs: CarSpecs = CarSpecs(mass=0., wheelbase=0., steerRatio=0.)
dbc_dict: DbcDict = field(default_factory=lambda: dict())
class PlatformsType(EnumType):
def __new__(metacls, cls, bases, classdict, *, boundary=None, _simple=False, **kwds):
for key in classdict._member_names.keys():
cfg: PlatformConfig = classdict[key]
cfg.platform_str = key
cfg.freeze()
return super().__new__(metacls, cls, bases, classdict, boundary=boundary, _simple=_simple, **kwds)
class Platforms(str, ReprEnum, metaclass=PlatformsType):
config: PlatformConfigBase
def __new__(cls, platform_config: PlatformConfig):
member = str.__new__(cls, platform_config.platform_str)
member.config = platform_config
member._value_ = platform_config.platform_str
return member
def __repr__(self):
return f"<{self.__class__.__name__}.{self.name}>"
@classmethod
def create_dbc_map(cls) -> dict[str, DbcDict]:
return {p: p.config.dbc_dict for p in cls}
@classmethod
def with_flags(cls, flags: IntFlag) -> set['Platforms']:
return {p for p in cls if p.config.flags & flags}
@classmethod
def with_iq_flags(cls, iq_flags: IntFlag) -> set['Platforms']:
return {p for p in cls if p.config.iq_flags & iq_flags}

0
artifacts/package_runtime/iqdbc/car/body/__init__.py Normal file
View File

17
artifacts/package_runtime/iqdbc/car/body/bodycan.py Normal file
View File

@@ -0,0 +1,17 @@
from iqdbc.car.crc import CRC8BODY
def create_control(packer, torque_l, torque_r):
values = {
"TORQUE_L": torque_l,
"TORQUE_R": torque_r,
}
return packer.make_can_msg("TORQUE_CMD", 0, values)
def body_checksum(address: int, sig, d: bytearray) -> int:
crc = 0xFF
for i in range(len(d) - 2, -1, -1):
crc = CRC8BODY[crc ^ d[i]]
return crc

82
artifacts/package_runtime/iqdbc/car/body/carcontroller.py Normal file
View File

@@ -0,0 +1,82 @@
import numpy as np
from iqdbc.can import CANPacker
from iqdbc.car import Bus, DT_CTRL
from iqdbc.car.common.pid import PIDController
from iqdbc.car.body import bodycan
from iqdbc.car.body.values import SPEED_FROM_RPM
from iqdbc.car.interfaces import CarControllerBase
MAX_TORQUE = 500
MAX_TORQUE_RATE = 50
MAX_ANGLE_ERROR = np.radians(7)
MAX_POS_INTEGRATOR = 0.2 # meters
MAX_TURN_INTEGRATOR = 0.1 # meters
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_IQ):
super().__init__(dbc_names, CP, CP_IQ)
self.packer = CANPacker(dbc_names[Bus.main])
# PIDs
self.turn_pid = PIDController(110, k_i=11.5, rate=1 / DT_CTRL)
self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1 / DT_CTRL)
self.torque_r_filtered = 0.
self.torque_l_filtered = 0.
@staticmethod
def deadband_filter(torque, deadband):
if torque > 0:
torque += deadband
else:
torque -= deadband
return torque
def update(self, CC, CC_IQ, CS, now_nanos):
torque_l = 0
torque_r = 0
if CC.enabled:
# Read these from the joystick
# TODO: this isn't acceleration, okay?
speed_desired = CC.actuators.accel / 5.
speed_diff_desired = -CC.actuators.torque / 2.
speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
speed_error = speed_desired - speed_measured
torque = self.wheeled_speed_pid.update(speed_error, freeze_integrator=False)
speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
turn_error = speed_diff_measured - speed_diff_desired
freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_TURN_INTEGRATOR) or
(turn_error > 0 and self.turn_pid.error_integral >= MAX_TURN_INTEGRATOR))
torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator)
# Combine 2 PIDs outputs
torque_r = torque + torque_diff
torque_l = torque - torque_diff
# Torque rate limits
self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10),
self.torque_r_filtered - MAX_TORQUE_RATE,
self.torque_r_filtered + MAX_TORQUE_RATE)
self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10),
self.torque_l_filtered - MAX_TORQUE_RATE,
self.torque_l_filtered + MAX_TORQUE_RATE)
torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE))
torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE))
can_sends = []
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r))
new_actuators = CC.actuators.as_builder()
new_actuators.accel = torque_l
new_actuators.torque = torque_r
new_actuators.torqueOutputCan = torque_r
self.frame += 1
return new_actuators, can_sends

36
artifacts/package_runtime/iqdbc/car/body/carstate.py Normal file
View File

@@ -0,0 +1,36 @@
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.interfaces import CarStateBase
from iqdbc.car.body.values import DBC
class CarState(CarStateBase):
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
cp = can_parsers[Bus.main]
ret = structs.CarState()
ret_iq = structs.IQCarState()
ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L']
ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R']
ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = False
ret.steerFaultPermanent = any([cp.vl['VAR_VALUES']['MOTOR_ERR_L'], cp.vl['VAR_VALUES']['MOTOR_ERR_R'],
cp.vl['VAR_VALUES']['FAULT']])
ret.charging = cp.vl["BODY_DATA"]["CHARGER_CONNECTED"] == 1
ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100
# irrelevant for non-car
ret.gearShifter = structs.CarState.GearShifter.drive
ret.cruiseState.enabled = True
ret.cruiseState.available = True
return ret, ret_iq
@staticmethod
def get_can_parsers(CP, CP_IQ):
return {Bus.main: CANParser(DBC[CP.carFingerprint][Bus.main], [], 0)}

28
artifacts/package_runtime/iqdbc/car/body/fingerprints.py Normal file
View File

@@ -0,0 +1,28 @@
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
from iqdbc.car.structs import CarParams
from iqdbc.car.body.values import CAR
Ecu = CarParams.Ecu
# debug ecu fw version is the git hash of the firmware
FINGERPRINTS = {
CAR.COMMA_BODY: [{
513: 8, 516: 8, 514: 3, 515: 4
}],
}
FW_VERSIONS = {
CAR.COMMA_BODY: {
(Ecu.engine, 0x720, None): [
b'0.0.01',
b'0.3.00a',
b'02/27/2022',
],
(Ecu.debug, 0x721, None): [
b'166bd860',
b'dc780f85',
],
},
}

30
artifacts/package_runtime/iqdbc/car/body/interface.py Normal file
View File

@@ -0,0 +1,30 @@
import math
from iqdbc.car import get_safety_config, structs
from iqdbc.car.body.carcontroller import CarController
from iqdbc.car.body.carstate import CarState
from iqdbc.car.body.values import SPEED_FROM_RPM
from iqdbc.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.notCar = True
ret.brand = "body"
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.body)]
ret.minSteerSpeed = -math.inf
ret.maxLateralAccel = math.inf # TODO: set to a reasonable value
ret.steerLimitTimer = 1.0
ret.steerActuatorDelay = 0.
ret.wheelSpeedFactor = SPEED_FROM_RPM
ret.radarUnavailable = True
ret.openpilotLongitudinalControl = True
ret.steerControlType = structs.CarParams.SteerControlType.angle
return ret

40
artifacts/package_runtime/iqdbc/car/body/values.py Normal file
View File

@@ -0,0 +1,40 @@
from iqdbc.car import Bus, CarSpecs, PlatformConfig, Platforms
from iqdbc.car.structs import CarParams
from iqdbc.car.docs_definitions import CarDocs
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = CarParams.Ecu
SPEED_FROM_RPM = 0.008587
class CarControllerParams:
ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [5., .8, .15] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
STEER_THRESHOLD = 1.0
def __init__(self, CP):
pass
class CAR(Platforms):
COMMA_BODY = PlatformConfig(
[CarDocs("comma body", package="All", video="https://youtu.be/VT-i3yRsX2s?t=2736")],
CarSpecs(mass=9, wheelbase=0.406, steerRatio=0.5, centerToFrontRatio=0.44),
{Bus.main: 'comma_body'},
)
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
bus=0,
),
],
)
DBC = CAR.create_dbc_map()

15
artifacts/package_runtime/iqdbc/car/can_definitions.py Normal file
View File

@@ -0,0 +1,15 @@
from collections.abc import Callable
from typing import NamedTuple, Protocol
class CanData(NamedTuple):
address: int
dat: bytes
src: int
CanSendCallable = Callable[[list[CanData]], None]
class CanRecvCallable(Protocol):
def __call__(self, wait_for_one: bool = False) -> list[list[CanData]]: ...

839
artifacts/package_runtime/iqdbc/car/car.capnp Normal file
View File

@@ -0,0 +1,839 @@
using Cxx = import "./include/c++.capnp";
$Cxx.namespace("cereal");
@0x8e2af1e708af8b8d;
# ******* events causing controls state machine transition *******
# IMPORTANT: This struct is to not be modified so old logs can be parsed
struct OnroadEventDEPRECATED @0x9b1657f34caf3ad3 {
name @0 :EventName;
# event types
enable @1 :Bool;
noEntry @2 :Bool;
warning @3 :Bool; # alerts presented only when enabled or soft disabling
userDisable @4 :Bool;
softDisable @5 :Bool;
immediateDisable @6 :Bool;
preEnable @7 :Bool;
permanent @8 :Bool; # alerts presented regardless of openpilot state
overrideLateral @10 :Bool;
overrideLongitudinal @9 :Bool;
enum EventName @0xbaa8c5d505f727de {
canError @0;
steerUnavailable @1;
wrongGear @4;
doorOpen @5;
seatbeltNotLatched @6;
espDisabled @7;
wrongCarMode @8;
steerTempUnavailable @9;
reverseGear @10;
buttonCancel @11;
buttonEnable @12;
pedalPressed @13; # exits active state
preEnableStandstill @73; # added during pre-enable state with brake
gasPressedOverride @108; # added when user is pressing gas with no disengage on gas
steerOverride @114;
cruiseDisabled @14;
speedTooLow @17;
outOfSpace @18;
overheat @19;
calibrationIncomplete @20;
calibrationInvalid @21;
calibrationRecalibrating @117;
controlsMismatch @22;
pcmEnable @23;
pcmDisable @24;
radarFault @26;
brakeHold @28;
parkBrake @29;
manualRestart @30;
joystickDebug @34;
longitudinalManeuver @124;
steerTempUnavailableSilent @35;
resumeRequired @36;
preDriverDistracted @37;
promptDriverDistracted @38;
driverDistracted @39;
preDriverUnresponsive @43;
promptDriverUnresponsive @44;
driverUnresponsive @45;
belowSteerSpeed @46;
lowBattery @48;
accFaulted @51;
sensorDataInvalid @52;
commIssue @53;
commIssueAvgFreq @109;
tooDistracted @54;
posenetInvalid @55;
preLaneChangeLeft @57;
preLaneChangeRight @58;
laneChange @59;
lowMemory @63;
stockAeb @64;
ldw @65;
carUnrecognized @66;
invalidLkasSetting @69;
speedTooHigh @70;
laneChangeBlocked @71;
relayMalfunction @72;
stockFcw @74;
startup @75;
startupNoCar @76;
startupNoControl @77;
startupNoSecOcKey @125;
startupMaster @78;
fcw @79;
steerSaturated @80;
belowEngageSpeed @84;
noGps @85;
wrongCruiseMode @87;
modeldLagging @89;
deviceFalling @90;
fanMalfunction @91;
cameraMalfunction @92;
cameraFrameRate @110;
processNotRunning @95;
dashcamMode @96;
selfdriveInitializing @98;
usbError @99;
cruiseMismatch @106;
canBusMissing @111;
selfdrivedLagging @112;
resumeBlocked @113;
steerTimeLimit @115;
vehicleSensorsInvalid @116;
locationdTemporaryError @103;
locationdPermanentError @118;
paramsdTemporaryError @50;
paramsdPermanentError @119;
actuatorsApiUnavailable @120;
espActive @121;
personalityChanged @122;
aeb @123;
radarCanErrorDEPRECATED @15;
communityFeatureDisallowedDEPRECATED @62;
radarCommIssueDEPRECATED @67;
driverMonitorLowAccDEPRECATED @68;
gasUnavailableDEPRECATED @3;
dataNeededDEPRECATED @16;
modelCommIssueDEPRECATED @27;
ipasOverrideDEPRECATED @33;
geofenceDEPRECATED @40;
driverMonitorOnDEPRECATED @41;
driverMonitorOffDEPRECATED @42;
calibrationProgressDEPRECATED @47;
invalidGiraffeHondaDEPRECATED @49;
invalidGiraffeToyotaDEPRECATED @60;
internetConnectivityNeededDEPRECATED @61;
whitePandaUnsupportedDEPRECATED @81;
commIssueWarningDEPRECATED @83;
focusRecoverActiveDEPRECATED @86;
neosUpdateRequiredDEPRECATED @88;
modelLagWarningDEPRECATED @93;
startupOneplusDEPRECATED @82;
startupFuzzyFingerprintDEPRECATED @97;
noTargetDEPRECATED @25;
brakeUnavailableDEPRECATED @2;
plannerErrorDEPRECATED @32;
gpsMalfunctionDEPRECATED @94;
roadCameraErrorDEPRECATED @100;
driverCameraErrorDEPRECATED @101;
wideRoadCameraErrorDEPRECATED @102;
highCpuUsageDEPRECATED @105;
startupNoFwDEPRECATED @104;
lowSpeedLockoutDEPRECATED @31;
lkasDisabledDEPRECATED @107;
soundsUnavailableDEPRECATED @56;
}
}
struct CarState {
# CAN health
canValid @26 :Bool; # invalid counter/checksums
canTimeout @40 :Bool; # CAN bus dropped out
canErrorCounter @48 :UInt32;
# process meta
cumLagMs @50 :Float32;
# car speed
vEgo @1 :Float32; # best estimate of speed
aEgo @16 :Float32; # best estimate of aCAN cceleration
vEgoRaw @17 :Float32; # unfiltered speed from wheel speed sensors
vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI
vCruise @53 :Float32; # actual set speed
vCruiseCluster @54 :Float32; # set speed to display in the UI
yawRate @22 :Float32; # best estimate of yaw rate
standstill @18 :Bool;
wheelSpeeds @2 :WheelSpeeds;
gasPressed @4 :Bool; # this is user pedal only
# brake pedal, 0.0-1.0
brake @5 :Float32; # this is user pedal only
brakePressed @6 :Bool; # this is user pedal only
regenBraking @45 :Bool; # this is user pedal only
parkingBrake @39 :Bool;
brakeHoldActive @38 :Bool;
# steering wheel
steeringAngleDeg @7 :Float32;
steeringAngleOffsetDeg @37 :Float32; # Offset between sensors in case there multiple
steeringRateDeg @15 :Float32; # optional
steeringTorque @8 :Float32; # Native CAN units, only needed on cars where it's used for control
steeringTorqueEps @27 :Float32; # Native CAN units, only needed on cars where it's used for control
steeringPressed @9 :Bool; # is the user overring the steering wheel?
steeringDisengage @58 :Bool; # more force than steeringPressed, disengages for applicable brands
steerFaultTemporary @35 :Bool;
steerFaultPermanent @36 :Bool;
steeringCurvature @64 :Float32;
invalidLkasSetting @55 :Bool; # stock LKAS is incorrectly configured (i.e. on or off)
stockAeb @30 :Bool;
stockLkas @59 :Bool;
stockFcw @31 :Bool;
espDisabled @32 :Bool;
accFaulted @42 :Bool;
carFaultedNonCritical @47 :Bool; # some ECU is faulted, but car remains controllable
espActive @51 :Bool;
vehicleSensorsInvalid @52 :Bool; # invalid steering angle readings, etc.
lowSpeedAlert @56 :Bool; # lost steering control due to a dynamic min steering speed
blockPcmEnable @60 :Bool; # whether to allow PCM to enable this frame
lateralAvailable @61 :Bool; # lateral control is available even if cruise is faulted
cruiseFaultLateralMode @62 :Bool; # cruise is faulted but lateral control is still active
carNotReady @95 :Bool; # car is transiently refusing engagement, not a fault
radarDisableFailed @66 :Bool;
# Physical vehicle odometer in kilometers. Zero means unavailable on this platform.
odometer @67 :Float64;
# cruise state
cruiseState @10 :CruiseState;
# gear
gearShifter @14 :GearShifter;
# button presses
buttonEvents @11 :List(ButtonEvent);
buttonEnable @57 :Bool; # user is requesting enable, usually one frame. set if pcmCruise=False
leftBlinker @20 :Bool;
rightBlinker @21 :Bool;
genericToggle @23 :Bool;
# lock info
doorOpen @24 :Bool; # ideally includes all doors
seatbeltUnlatched @25 :Bool; # driver seatbelt
# blindspot sensors
leftBlindspot @33 :Bool; # Is there something blocking the left lane change
rightBlindspot @34 :Bool; # Is there something blocking the right lane change
fuelGauge @41 :Float32; # battery or fuel tank level from [0.0, 1.0]
charging @43 :Bool;
fuelTankLevelL @63 :Float32; # raw fuel tank level in liters (konn3kt: VW PQ Kombi_1.Tankinhalt)
batteryDetails @65 :BatteryDetails;
# carrotpilot HKG extension state
vCluRatio @68 :Float32;
logCarrot @69 :Text;
softHoldActive @70 :Int16;
activateCruise @71 :Int16;
latEnabled @72 :Bool;
pcmCruiseGap @73 :Int16;
speedLimit @74 :Float32;
speedLimitDistance @75 :Float32;
gearStep @76 :Int16;
tpms @77 :Tpms;
useLaneLineSpeed @78 :Float32;
leftLatDist @79 :Float32;
rightLatDist @80 :Float32;
leftLongDist @81 :Float32;
rightLongDist @82 :Float32;
carrotCruise @83 :Int16;
leftLaneLine @84 :Int16;
rightLaneLine @85 :Int16;
datetime @86 :UInt64;
leftRearLongDist @87 :Float32;
rightRearLongDist @88 :Float32;
leftRearLatDist @89 :Float32;
rightRearLatDist @90 :Float32;
trailerConnected @91 :Bool;
ureaGauge @92 :Float32;
evModeActive @93 :Bool;
evModeValid @94 :Bool;
struct Tpms {
fl @0 :Float32;
fr @1 :Float32;
rl @2 :Float32;
rr @3 :Float32;
}
struct BatteryDetails {
capacity @0 :Float32;
charge @1 :Float32;
soc @2 :Float32;
temperature @3 :Float32;
heaterActive @4 :Bool;
voltage @5 :Float32;
current @6 :Float32;
power @7 :Float32;
chargingMode @8 :UInt8;
}
struct WheelSpeeds {
# optional wheel speeds
fl @0 :Float32;
fr @1 :Float32;
rl @2 :Float32;
rr @3 :Float32;
}
struct CruiseState {
enabled @0 :Bool;
speed @1 :Float32;
speedCluster @6 :Float32; # Set speed as shown on instrument cluster
available @2 :Bool;
standstill @4 :Bool;
nonAdaptive @5 :Bool;
speedLimit @7 :Float32;
speedLimitPredicative @8 :Float32;
speedOffsetDEPRECATED @3 :Float32;
}
enum GearShifter {
unknown @0;
park @1;
drive @2;
neutral @3;
reverse @4;
sport @5;
low @6;
brake @7;
eco @8;
manumatic @9;
}
# send on change
struct ButtonEvent {
pressed @0 :Bool;
type @1 :Type;
enum Type {
unknown @0;
leftBlinker @1;
rightBlinker @2;
accelCruise @3;
decelCruise @4;
cancel @5;
lkas @6;
altButton2 @7;
mainCruise @8;
setCruise @9;
resumeCruise @10;
gapAdjustCruise @11;
lfaButton @12;
paddleLeft @13;
paddleRight @14;
}
}
# deprecated
errorsDEPRECATED @0 :List(OnroadEventDEPRECATED.EventName);
gas @3 :Float32; # this is user pedal only
brakeLights @19 :Bool;
steeringRateLimitedDEPRECATED @29 :Bool;
canMonoTimesDEPRECATED @12: List(UInt64);
canRcvTimeoutDEPRECATED @49 :Bool;
eventsDEPRECATED @13 :List(OnroadEventDEPRECATED);
clutchPressedDEPRECATED @28 :Bool;
engineRpmDEPRECATED @46 :Float32;
}
# ******* radar state @ 20hz *******
struct RadarData @0x888ad6581cf0aacb {
errors @3 :Error;
points @1 :List(RadarPoint);
struct Error {
canError @0 :Bool;
radarFault @1 :Bool;
wrongConfig @2 :Bool;
radarUnavailableTemporary @3 :Bool; # radar data is temporarily unavailable due to conditions the car sets
}
# similar to LiveTracks
# is one timestamp valid for all? I think so
struct RadarPoint {
trackId @0 :UInt64; # no trackId reuse
# these 3 are the minimum required
dRel @1 :Float32; # m from the front bumper of the car
yRel @2 :Float32; # m
vRel @3 :Float32; # m/s
# these are optional and valid if they are not NaN
aRel @4 :Float32; # m/s^2
yvRel @5 :Float32; # m/s
# some radars flag measurements VS estimates
measured @6 :Bool;
vLead @7 :Float32; # m/s
aLead @8 :Float32; # m/s^2
jLead @9 :Float32; # m/s^3
radarSource @10 :RadarSource;
enum RadarSource {
frontRadar @0;
scc @1;
corner235 @2;
corner180 @3;
}
}
enum ErrorDEPRECATED {
canError @0;
fault @1;
wrongConfig @2;
}
# deprecated
canMonoTimesDEPRECATED @2 :List(UInt64);
errorsDEPRECATED @0 :List(ErrorDEPRECATED);
}
# ******* car controls @ 100hz *******
struct CarControl {
# must be true for any actuator commands to work
enabled @0 :Bool;
latActive @11: Bool;
longActive @12: Bool;
# Final actuator commands
actuators @6 :Actuators;
# Blinker controls
leftBlinker @15: Bool;
rightBlinker @16: Bool;
orientationNED @13 :List(Float32);
angularVelocity @14 :List(Float32);
currentCurvature @17 :Float32; # From vehicle model
curvatureControllerActive @18: Bool;
rollCompensation @19 :Float32;
steerLimited @20: Bool;
forceRHDForBSM @21: Bool;
longComfortMode @22: Bool;
cruiseControl @4 :CruiseControl;
hudControl @5 :HUDControl;
struct Actuators {
# lateral commands, mutually exclusive
torque @2: Float32; # [0.0, 1.0]
steeringAngleDeg @3: Float32;
curvature @7: Float32;
# longitudinal commands
accel @4: Float32; # m/s^2
longControlState @5: LongControlState;
# these are only for logging the actual values sent to the car over CAN
gas @0: Float32; # [0.0, 1.0]
brake @1: Float32; # [0.0, 1.0]
torqueOutputCan @8: Float32; # value sent over can to the car
speed @6: Float32; # m/s
jerk @9: Float32; # m/s^3
aTarget @10: Float32; # m/s^2
enum LongControlState @0xe40f3a917d908282{
off @0;
pid @1;
stopping @2;
starting @3;
}
}
struct CruiseControl {
cancel @0: Bool;
resume @1: Bool;
override @4: Bool;
speedLimit @5: Bool;
speedLimitPredicative @6: Bool;
speedLimitPredReactToSL @7: Bool;
speedLimitPredReactToCurves @8: Bool;
speedOverrideDEPRECATED @2: Float32;
accelOverrideDEPRECATED @3: Float32;
}
struct HUDControl {
speedVisible @0: Bool;
setSpeed @1: Float32;
lanesVisible @2: Bool;
leadVisible @3: Bool;
visualAlert @4: VisualAlert;
rightLaneVisible @6: Bool;
leftLaneVisible @7: Bool;
rightLaneDepart @8: Bool;
leftLaneDepart @9: Bool;
leadDistanceBars @10: Int8; # 1-3: 1 is closest, 3 is farthest. some ports may utilize 2-4 bars instead
leadFollowTime @11: Float32;
leadDistance @12: Float32;
driverUnresponsive @13: Bool;
activeCarrot @14: Int16;
leadRelSpeed @15: Float32;
leadDPath @16: Float32;
leadRadar @17: Int16;
modelDesire @18: Int16;
atcDistance @19: Float32;
# not used with the dash, TODO: separate structs for dash UI and device UI
audibleAlert @5: AudibleAlert;
enum VisualAlert {
# these are the choices from the Honda
# map as good as you can for your car
none @0;
fcw @1;
steerRequired @2;
brakePressed @3;
wrongGear @4;
seatbeltUnbuckled @5;
speedTooHigh @6;
ldw @7;
}
enum AudibleAlert {
none @0;
engage @1;
disengage @2;
refuse @3;
warningSoft @4;
warningImmediate @5;
prompt @6;
promptRepeat @7;
promptDistracted @8;
preAlert @9;
}
}
gasDEPRECATED @1 :Float32;
brakeDEPRECATED @2 :Float32;
steeringTorqueDEPRECATED @3 :Float32;
activeDEPRECATED @7 :Bool;
rollDEPRECATED @8 :Float32;
pitchDEPRECATED @9 :Float32;
actuatorsOutputDEPRECATED @10 :Actuators;
}
struct CarOutput {
# Any car specific rate limits or quirks applied by
# the CarController are reflected in actuatorsOutput
# and matches what is sent to the car
actuatorsOutput @0 :CarControl.Actuators;
}
# ****** car param ******
struct CarParams {
brand @0 :Text; # Designates which group a platform falls under. Each folder in iqdbc/car is assigned one brand string
carFingerprint @1 :Text;
fuzzyFingerprint @55 :Bool;
notCar @66 :Bool; # flag for non-car robotics platforms
pcmCruise @3 :Bool; # is openpilot's state tied to the PCM's cruise state?
enableBsm @56 :Bool; # blind spot monitoring
flags @64 :UInt32; # flags for car specific quirks
alphaLongitudinalAvailable @71 :Bool;
extFlags @79 :UInt32; # carrotpilot HKG extension flags
minEnableSpeed @7 :Float32;
minSteerSpeed @8 :Float32;
steerAtStandstill @77 :Bool; # is steering available at standstill? just check if it faults
safetyConfigs @62 :List(SafetyConfig);
alternativeExperience @65 :Int16; # panda flag for features like no disengage on gas
# Car docs fields, not used for control
maxLateralAccel @68 :Float32;
autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically
# things about the car in the manual
mass @17 :Float32; # [kg] curb weight: all fluids no cargo
wheelbase @18 :Float32; # [m] distance from rear axle to front axle
centerToFront @19 :Float32; # [m] distance from center of mass to front axle
steerRatio @20 :Float32; # [] ratio of steering wheel angle to front wheel angle
steerRatioRear @21 :Float32; # [] ratio of steering wheel angle to rear wheel angle (usually 0)
# things we can derive
rotationalInertia @22 :Float32; # [kg*m2] body rotational inertia
tireStiffnessFactor @72 :Float32; # scaling factor used in calculating tireStiffness[Front,Rear]
tireStiffnessFront @23 :Float32; # [N/rad] front tire coeff of stiff
tireStiffnessRear @24 :Float32; # [N/rad] rear tire coeff of stiff
longitudinalTuning @25 :LongitudinalPIDTuning;
lateralParams @48 :LateralParams;
lateralTuning :union {
pid @26 :LateralPIDTuning;
indiDEPRECATED @27 :LateralINDITuning;
lqrDEPRECATED @40 :LateralLQRTuning;
torque @67 :LateralTorqueTuning;
}
steerLimitAlert @28 :Bool;
steerLimitTimer @47 :Float32; # time before steerLimitAlert is issued
steerControlType @34 :SteerControlType;
radarUnavailable @35 :Bool; # True when radar objects aren't visible on CAN or aren't parsed out
stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary
steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
longitudinalActuatorDelay @58 :Float32; # Gas/Brake actuator delay in seconds
openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?
carVin @38 :Text; # VIN number queried during fingerprinting
dashcamOnly @41: Bool;
dashcamOnlyReason @78 :DashcamOnlyReason;
passive @73: Bool; # is openpilot in control?
transmissionType @43 :TransmissionType;
carFw @44 :List(CarFw);
radarDelay @74 :Float32;
fingerprintSource @49: FingerprintSource;
networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network
wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds
secOcRequired @75 :Bool; # Car requires SecOC message authentication to operate
secOcKeyAvailable @76 :Bool; # Stored SecOC key loaded from params
struct SafetyConfig {
safetyModel @0 :SafetyModel;
safetyParam @3 :UInt16;
safetyParamDEPRECATED @1 :Int16;
safetyParam2DEPRECATED @2 :UInt32;
}
struct LateralParams {
torqueBP @0 :List(Int32);
torqueV @1 :List(Int32);
}
struct LateralPIDTuning {
kpBP @0 :List(Float32);
kpV @1 :List(Float32);
kiBP @2 :List(Float32);
kiV @3 :List(Float32);
kf @4 :Float32;
}
struct LateralTorqueTuning {
friction @3 :Float32;
steeringAngleDeadzoneDeg @5 :Float32;
latAccelFactor @6 :Float32;
latAccelOffset @7 :Float32;
useSteeringAngleDEPRECATED @0 :Bool;
kpDEPRECATED @1 :Float32;
kiDEPRECATED @2 :Float32;
kfDEPRECATED @4 :Float32;
kdDEPRECATED @8 : Float32;
}
struct LongitudinalPIDTuning {
kpBP @0 :List(Float32);
kpV @1 :List(Float32);
kiBP @2 :List(Float32);
kiV @3 :List(Float32);
kf @6 :Float32;
deadzoneBPDEPRECATED @4 :List(Float32);
deadzoneVDEPRECATED @5 :List(Float32);
}
struct LateralINDITuning {
outerLoopGainBP @4 :List(Float32);
outerLoopGainV @5 :List(Float32);
innerLoopGainBP @6 :List(Float32);
innerLoopGainV @7 :List(Float32);
timeConstantBP @8 :List(Float32);
timeConstantV @9 :List(Float32);
actuatorEffectivenessBP @10 :List(Float32);
actuatorEffectivenessV @11 :List(Float32);
outerLoopGainDEPRECATED @0 :Float32;
innerLoopGainDEPRECATED @1 :Float32;
timeConstantDEPRECATED @2 :Float32;
actuatorEffectivenessDEPRECATED @3 :Float32;
}
struct LateralLQRTuning {
scale @0 :Float32;
ki @1 :Float32;
dcGain @2 :Float32;
# State space system
a @3 :List(Float32);
b @4 :List(Float32);
c @5 :List(Float32);
k @6 :List(Float32); # LQR gain
l @7 :List(Float32); # Kalman gain
}
enum SafetyModel {
silent @0;
hondaNidec @1;
toyota @2;
elm327 @3;
gm @4;
hondaBoschGiraffe @5;
ford @6;
cadillac @7;
hyundai @8;
chrysler @9;
tesla @10;
subaru @11;
gmPassive @12;
mazda @13;
nissan @14;
volkswagen @15;
toyotaIpas @16;
allOutput @17;
gmAscm @18;
noOutput @19; # like silent but without silent CAN TXs
hondaBosch @20;
volkswagenPq @21;
subaruPreglobal @22; # pre-Global platform
hyundaiLegacy @23;
hyundaiCommunity @24;
volkswagenMlb @25;
hongqi @26;
body @27;
hyundaiCanfd @28;
volkswagenMqbEvo @29;
chryslerCusw @30;
psa @31;
fcaGiorgio @32;
rivian @33;
volkswagenMeb @34;
}
enum SteerControlType {
torque @0;
angle @1;
curvatureDEPRECATED @2;
}
enum TransmissionType {
unknown @0;
automatic @1; # Traditional auto, including DSG
manual @2; # True "stick shift" only
direct @3; # Electric vehicle or other direct drive
cvt @4;
}
enum DashcamOnlyReason {
unknown @0;
radarDisableEngineOn @1;
}
struct CarFw {
ecu @0 :Ecu;
fwVersion @1 :Data;
address @2 :UInt32;
subAddress @3 :UInt8;
responseAddress @4 :UInt32;
request @5 :List(Data);
brand @6 :Text;
bus @7 :UInt8;
logging @8 :Bool;
obdMultiplexing @9 :Bool;
}
enum Ecu {
eps @0;
abs @1;
fwdRadar @2;
fwdCamera @3;
engine @4;
unknown @5;
transmission @8; # Transmission Control Module
hybrid @18; # hybrid control unit, e.g. Chrysler's HCP, Honda's IMA Control Unit, Toyota's hybrid control computer
inverter @25; # inverter for electric engine
srs @9; # airbag
gateway @10; # can gateway
hud @11; # heads up display
combinationMeter @12; # instrument cluster
electricBrakeBooster @15;
shiftByWire @16;
adas @19;
cornerRadar @21;
hvac @20;
parkingAdas @7; # parking assist system ECU, e.g. Toyota's IPAS, Hyundai's RSPA, etc.
epb @22; # electronic parking brake
telematics @23;
body @24; # body control module
# Toyota only
dsu @6;
# Honda only
vsa @13; # Vehicle Stability Assist
programmedFuelInjection @14;
debug @17;
}
enum FingerprintSource {
can @0;
fw @1;
fixed @2;
}
enum NetworkLocation {
fwdCamera @0; # Standard/default integration at LKAS camera
gateway @1; # Integration at vehicle's CAN gateway
}
enableGasInterceptorDEPRECATED @2 :Bool;
enableCameraDEPRECATED @4 :Bool;
enableApgsDEPRECATED @6 :Bool;
steerRateCostDEPRECATED @33 :Float32;
isPandaBlackDEPRECATED @39 :Bool;
hasStockCameraDEPRECATED @57 :Bool;
safetyParamDEPRECATED @10 :Int16;
safetyModelDEPRECATED @9 :SafetyModel;
safetyModelPassiveDEPRECATED @42 :SafetyModel = silent;
minSpeedCanDEPRECATED @51 :Float32;
communityFeatureDEPRECATED @46: Bool;
startingAccelRateDEPRECATED @53 :Float32;
steerMaxBPDEPRECATED @11 :List(Float32);
steerMaxVDEPRECATED @12 :List(Float32);
gasMaxBPDEPRECATED @13 :List(Float32);
gasMaxVDEPRECATED @14 :List(Float32);
brakeMaxBPDEPRECATED @15 :List(Float32);
brakeMaxVDEPRECATED @16 :List(Float32);
directAccelControlDEPRECATED @30 :Bool;
maxSteeringAngleDegDEPRECATED @54 :Float32;
longitudinalActuatorDelayLowerBoundDEPRECATED @61 :Float32;
stoppingControlDEPRECATED @31 :Bool; # Does the car allow full control even at lows speeds when stopping
radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard
enableDsuDEPRECATED @5 :Bool; # driving support unit
vEgoStarting @59 :Float32;
startAccel @32 :Float32;
startingState @70 :Bool;
vEgoStoppingDEPRECATED @29 :Float32;
stoppingDecelRateDEPRECATED @52 :Float32;
}

181
artifacts/package_runtime/iqdbc/car/car_helpers.py Normal file
View File

@@ -0,0 +1,181 @@
import os
import time
from iqdbc.car import gen_empty_fingerprint
from iqdbc.car.can_definitions import CanRecvCallable, CanSendCallable
from iqdbc.car.carlog import carlog
from iqdbc.car.structs import CarParams, CarParamsT
from iqdbc.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from iqdbc.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car
from iqdbc.car.mock.values import CAR as MOCK
from iqdbc.car.values import BRANDS
from iqdbc.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
from iqdbc.lvbs.car.interfaces import apply_iq_car_config as iqpilot_interfaces
FRAME_FINGERPRINT = 100 # 1s
def load_interfaces(brand_names):
ret = {}
for brand_name in brand_names:
path = f'iqdbc.car.{brand_name}'
CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface
for model_name in brand_names[brand_name]:
ret[model_name] = CarInterface
return ret
def _get_interface_names() -> dict[str, list[str]]:
# returns a dict of brand name and its respective models
brand_names = {}
for brand in BRANDS:
brand_name = brand.__module__.split('.')[-2]
brand_names[brand_name] = [model.value for model in brand]
return brand_names
# imports from directory iqdbc/car/<name>/
interface_names = _get_interface_names()
interfaces = load_interfaces(interface_names)
def can_fingerprint(can_recv: CanRecvCallable) -> tuple[str | None, dict[int, dict]]:
finger = gen_empty_fingerprint()
candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
frame = 0
car_fingerprint = None
done = False
while not done:
# can_recv(wait_for_one=True) may return zero or multiple packets, so we increment frame for each one we receive
can_packets = can_recv(wait_for_one=True)
for can_packet in can_packets:
for can in can_packet:
# The fingerprint dict is generated for all buses, this way the car interface
# can use it to detect a (valid) multipanda setup and initialize accordingly
if can.src < 128:
if can.src not in finger:
finger[can.src] = {}
finger[can.src][can.address] = len(can.dat)
for b in candidate_cars:
# Ignore extended messages and VIN query response.
if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8):
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
if len(candidate_cars[b]) == 1 and frame > FRAME_FINGERPRINT:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# bail if no cars left or we've been waiting for more than 2s
failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > FRAME_FINGERPRINT) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
return car_fingerprint, finger
# **** for use live only ****
def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int,
cached_params: CarParamsT | None,
fixed_fingerprint: str | None) -> tuple[str | None, dict, str, list[CarParams.CarFw], CarParams.FingerprintSource, bool]:
fixed_fingerprint = fixed_fingerprint or os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) or bool(fixed_fingerprint)
disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False)
ecu_rx_addrs = set()
start_time = time.monotonic()
if not skip_fw_query:
if cached_params is not None and cached_params.brand != "mock" and len(cached_params.carFw) > 0 and \
cached_params.carVin is not VIN_UNKNOWN and not disable_fw_cache:
carlog.warning("Using cached CarParams")
vin_rx_addr, vin_rx_bus, vin = -1, -1, cached_params.carVin
car_fw = list(cached_params.carFw)
cached = True
else:
carlog.warning("Getting VIN & FW versions")
# enable OBD multiplexing for VIN query
# NOTE: this takes ~0.1s and is relied on to allow sendcan subscriber to connect in time
set_obd_multiplexing(True)
# VIN query only reliably works through OBDII
vin_rx_addr, vin_rx_bus, vin = get_vin(can_recv, can_send, (0, 1))
ecu_rx_addrs = get_present_ecus(can_recv, can_send, set_obd_multiplexing, num_pandas=num_pandas)
car_fw = get_fw_versions_ordered(can_recv, can_send, set_obd_multiplexing, vin, ecu_rx_addrs, num_pandas=num_pandas)
cached = False
exact_fw_match, fw_candidates = match_fw_to_car(car_fw, vin)
else:
vin_rx_addr, vin_rx_bus, vin = -1, -1, VIN_UNKNOWN
exact_fw_match, fw_candidates, car_fw = True, set(), []
cached = False
if not is_valid_vin(vin):
carlog.error({"event": "Malformed VIN", "vin": vin})
vin = VIN_UNKNOWN
carlog.warning("VIN %s", vin)
# disable OBD multiplexing for CAN fingerprinting and potential ECU knockouts
set_obd_multiplexing(False)
fw_query_time = time.monotonic() - start_time
# CAN fingerprint
# drain CAN socket so we get the latest messages
can_recv()
car_fingerprint, finger = can_fingerprint(can_recv)
exact_match = True
source = CarParams.FingerprintSource.can
# If FW query returns exactly 1 candidate, use it
if len(fw_candidates) == 1:
car_fingerprint = list(fw_candidates)[0]
source = CarParams.FingerprintSource.fw
exact_match = exact_fw_match
if fixed_fingerprint:
car_fingerprint = fixed_fingerprint
source = CarParams.FingerprintSource.fixed
carlog.error({"event": "fingerprinted", "car_fingerprint": str(car_fingerprint), "source": source, "fuzzy": not exact_match,
"cached": cached, "fw_count": len(car_fw), "ecu_responses": list(ecu_rx_addrs), "vin_rx_addr": vin_rx_addr,
"vin_rx_bus": vin_rx_bus, "fingerprints": repr(finger), "fw_query_time": fw_query_time})
return car_fingerprint, finger, vin, car_fw, source, exact_match
def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, alpha_long_allowed: bool,
is_release: bool, num_pandas: int = 1, cached_params: CarParamsT | None = None,
fixed_fingerprint: str | None = None, init_params_list_iq: list[dict[str, str]] | None = None, is_release_iq: bool = False):
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(can_recv, can_send, set_obd_multiplexing, num_pandas, cached_params,
fixed_fingerprint)
if candidate is None:
carlog.error({"event": "car doesn't match any fingerprints", "fingerprints": repr(fingerprints)})
candidate = "MOCK"
CarInterface = interfaces[candidate]
CP: CarParams = CarInterface.get_params(candidate, fingerprints, car_fw, alpha_long_allowed, is_release, docs=False)
CP.carVin = vin
CP.carFw = car_fw
CP.fingerprintSource = source
CP.fuzzyFingerprint = not exact_match
CP_IQ = CarInterface.get_params_iq(CP, candidate, fingerprints, car_fw, alpha_long_allowed, is_release_iq, docs=False)
iqpilot_interfaces(CarInterface, CP, CP_IQ, init_params_list_iq, can_recv, can_send)
return interfaces[CP.carFingerprint](CP, CP_IQ)
def get_demo_car_params():
platform = MOCK.MOCK
CarInterface = interfaces[platform]
CP = CarInterface.get_non_essential_params(platform)
return CP

12
artifacts/package_runtime/iqdbc/car/carlog.py Normal file
View File

@@ -0,0 +1,12 @@
import os
import logging
# set up logging
LOGPRINT = os.environ.get('LOGPRINT', 'INFO').upper()
carlog = logging.getLogger('carlog')
carlog.setLevel(LOGPRINT)
carlog.propagate = False
handler = logging.StreamHandler()
handler.setFormatter(logging.Formatter('%(message)s'))
carlog.addHandler(handler)

385
artifacts/package_runtime/iqdbc/car/ccp.py Normal file
View File

@@ -0,0 +1,385 @@
import sys
import time
import struct
from enum import IntEnum, Enum
from dataclasses import dataclass
@dataclass
class ExchangeStationIdsReturn:
id_length: int
data_type: int
available: int
protected: int
@dataclass
class GetDaqListSizeReturn:
list_size: int
first_pid: int
@dataclass
class GetSessionStatusReturn:
status: int
info: int | None
@dataclass
class DiagnosticServiceReturn:
length: int
type: int
@dataclass
class ActionServiceReturn:
length: int
type: int
class COMMAND_CODE(IntEnum):
CONNECT = 0x01
SET_MTA = 0x02
DNLOAD = 0x03
UPLOAD = 0x04
TEST = 0x05
START_STOP = 0x06
DISCONNECT = 0x07
START_STOP_ALL = 0x08
GET_ACTIVE_CAL_PAGE = 0x09
SET_S_STATUS = 0x0C
GET_S_STATUS = 0x0D
BUILD_CHKSUM = 0x0E
SHORT_UP = 0x0F
CLEAR_MEMORY = 0x10
SELECT_CAL_PAGE = 0x11
GET_SEED = 0x12
UNLOCK = 0x13
GET_DAQ_SIZE = 0x14
SET_DAQ_PTR = 0x15
WRITE_DAQ = 0x16
EXCHANGE_ID = 0x17
PROGRAM = 0x18
MOVE = 0x19
GET_CCP_VERSION = 0x1B
DIAG_SERVICE = 0x20
ACTION_SERVICE = 0x21
PROGRAM_6 = 0x22
DNLOAD_6 = 0x23
COMMAND_RETURN_CODES = {
0x00: "acknowledge / no error",
0x01: "DAQ processor overload",
0x10: "command processor busy",
0x11: "DAQ processor busy",
0x12: "internal timeout",
0x18: "key request",
0x19: "session status request",
0x20: "cold start request",
0x21: "cal. data init. request",
0x22: "DAQ list init. request",
0x23: "code update request",
0x30: "unknown command",
0x31: "command syntax",
0x32: "parameter(s) out of range",
0x33: "access denied",
0x34: "overload",
0x35: "access locked",
0x36: "resource/function not available",
}
class BYTE_ORDER(Enum):
LITTLE_ENDIAN = '<'
BIG_ENDIAN = '>'
class CommandTimeoutError(Exception):
pass
class CommandCounterError(Exception):
pass
class CommandResponseError(Exception):
def __init__(self, message, return_code):
super().__init__()
self.message = message
self.return_code = return_code
def __str__(self):
return self.message
class CcpClient:
def __init__(self, panda, tx_addr: int, rx_addr: int, bus: int=0, byte_order: BYTE_ORDER=BYTE_ORDER.BIG_ENDIAN, debug=False):
self.tx_addr = tx_addr
self.rx_addr = rx_addr
self.can_bus = bus
self.byte_order = byte_order
self.debug = debug
self._panda = panda
self._command_counter = -1
def _send_cro(self, cmd: int, dat: bytes = b"") -> None:
self._command_counter = (self._command_counter + 1) & 0xFF
tx_data = (bytes([cmd, self._command_counter]) + dat).ljust(8, b"\x00")
if self.debug:
print(f"CAN-TX: {hex(self.tx_addr)} - 0x{bytes.hex(tx_data)}")
assert len(tx_data) == 8, "data is not 8 bytes"
self._panda.can_clear(self.can_bus)
self._panda.can_clear(0xFFFF)
self._panda.can_send(self.tx_addr, tx_data, self.can_bus)
def _recv_dto(self, timeout: float) -> bytes:
start_time = time.time()
while time.time() - start_time < timeout:
msgs = self._panda.can_recv() or []
if len(msgs) >= 256:
print("CAN RX buffer overflow!!!", file=sys.stderr)
for rx_addr, rx_data_bytearray, rx_bus in msgs:
if rx_bus == self.can_bus and rx_addr == self.rx_addr:
rx_data = bytes(rx_data_bytearray)
if self.debug:
print(f"CAN-RX: {hex(rx_addr)} - 0x{bytes.hex(rx_data)}")
assert len(rx_data) == 8, f"message length not 8: {len(rx_data)}"
pid = rx_data[0]
if pid == 0xFF or pid == 0xFE:
err = rx_data[1]
err_desc = COMMAND_RETURN_CODES.get(err, "unknown error")
ctr = rx_data[2]
dat = rx_data[3:]
if pid == 0xFF and self._command_counter != ctr:
raise CommandCounterError(f"counter invalid: {ctr} != {self._command_counter}")
if err >= 0x10 and err <= 0x12:
if self.debug:
print(f"CCP-WAIT: {hex(err)} - {err_desc}")
start_time = time.time()
continue
if err >= 0x30:
raise CommandResponseError(f"{hex(err)} - {err_desc}", err)
else:
dat = rx_data[1:]
return dat
time.sleep(0.001)
raise CommandTimeoutError("timeout waiting for response")
# commands
def connect(self, station_addr: int) -> None:
if station_addr > 65535:
raise ValueError("station address must be less than 65536")
# NOTE: station address is always little endian
self._send_cro(COMMAND_CODE.CONNECT, struct.pack("<H", station_addr))
self._recv_dto(0.025)
def exchange_station_ids(self, device_id_info: bytes = b"") -> ExchangeStationIdsReturn:
self._send_cro(COMMAND_CODE.EXCHANGE_ID, device_id_info)
resp = self._recv_dto(0.025)
return ExchangeStationIdsReturn(id_length=resp[0], data_type=resp[1], available=resp[2], protected=resp[3])
def get_seed(self, resource_mask: int) -> bytes:
if resource_mask > 255:
raise ValueError("resource mask must be less than 256")
self._send_cro(COMMAND_CODE.GET_SEED, bytes([resource_mask]))
resp = self._recv_dto(0.025)
# protected = resp[0] == 0
seed = resp[1:]
return seed
def unlock(self, key: bytes) -> int:
if len(key) > 6:
raise ValueError("max key size is 6 bytes")
self._send_cro(COMMAND_CODE.UNLOCK, key)
resp = self._recv_dto(0.025)
status = resp[0]
return status
def set_memory_transfer_address(self, mta_num: int, addr_ext: int, addr: int) -> None:
if mta_num > 255:
raise ValueError("MTA number must be less than 256")
if addr_ext > 255:
raise ValueError("address extension must be less than 256")
self._send_cro(COMMAND_CODE.SET_MTA, bytes([mta_num, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
self._recv_dto(0.025)
def download(self, data: bytes) -> int:
if len(data) > 5:
raise ValueError("max data size is 5 bytes")
self._send_cro(COMMAND_CODE.DNLOAD, bytes([len(data)]) + data)
resp = self._recv_dto(0.025)
# mta_addr_ext = resp[0]
mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
return mta_addr
def download_6_bytes(self, data: bytes) -> int:
if len(data) != 6:
raise ValueError("data size must be 6 bytes")
self._send_cro(COMMAND_CODE.DNLOAD_6, data)
resp = self._recv_dto(0.025)
# mta_addr_ext = resp[0]
mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
return mta_addr
def upload(self, size: int) -> bytes:
if size > 5:
raise ValueError("size must be less than 6")
self._send_cro(COMMAND_CODE.UPLOAD, bytes([size]))
return self._recv_dto(0.025)[:size]
def short_upload(self, size: int, addr_ext: int, addr: int) -> bytes:
if size > 5:
raise ValueError("size must be less than 6")
if addr_ext > 255:
raise ValueError("address extension must be less than 256")
self._send_cro(COMMAND_CODE.SHORT_UP, bytes([size, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
return self._recv_dto(0.025)[:size]
def select_calibration_page(self) -> None:
self._send_cro(COMMAND_CODE.SELECT_CAL_PAGE)
self._recv_dto(0.025)
def get_daq_list_size(self, list_num: int, can_id: int = 0) -> GetDaqListSizeReturn:
if list_num > 255:
raise ValueError("list number must be less than 256")
self._send_cro(COMMAND_CODE.GET_DAQ_SIZE, bytes([list_num, 0]) + struct.pack(f"{self.byte_order.value}I", can_id))
resp = self._recv_dto(0.025)
return GetDaqListSizeReturn(list_size=resp[0], first_pid=resp[1])
def set_daq_list_pointer(self, list_num: int, odt_num: int, element_num: int) -> None:
if list_num > 255:
raise ValueError("list number must be less than 256")
if odt_num > 255:
raise ValueError("ODT number must be less than 256")
if element_num > 255:
raise ValueError("element number must be less than 256")
self._send_cro(COMMAND_CODE.SET_DAQ_PTR, bytes([list_num, odt_num, element_num]))
self._recv_dto(0.025)
def write_daq_list_entry(self, size: int, addr_ext: int, addr: int) -> None:
if size > 255:
raise ValueError("size must be less than 256")
if addr_ext > 255:
raise ValueError("address extension must be less than 256")
self._send_cro(COMMAND_CODE.WRITE_DAQ, bytes([size, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
self._recv_dto(0.025)
def start_stop_transmission(self, mode: int, list_num: int, odt_num: int, channel_num: int, rate_prescaler: int = 0) -> None:
if mode > 255:
raise ValueError("mode must be less than 256")
if list_num > 255:
raise ValueError("list number must be less than 256")
if odt_num > 255:
raise ValueError("ODT number must be less than 256")
if channel_num > 255:
raise ValueError("channel number must be less than 256")
if rate_prescaler > 65535:
raise ValueError("rate prescaler must be less than 65536")
self._send_cro(COMMAND_CODE.START_STOP, bytes([mode, list_num, odt_num, channel_num]) + struct.pack(f"{self.byte_order.value}H", rate_prescaler))
self._recv_dto(0.025)
def disconnect(self, station_addr: int, temporary: bool = False) -> None:
if station_addr > 65535:
raise ValueError("station address must be less than 65536")
# NOTE: station address is always little endian
self._send_cro(COMMAND_CODE.DISCONNECT, bytes([int(not temporary), 0x00]) + struct.pack("<H", station_addr))
self._recv_dto(0.025)
def set_session_status(self, status: int) -> None:
if status > 255:
raise ValueError("status must be less than 256")
self._send_cro(COMMAND_CODE.SET_S_STATUS, bytes([status]))
self._recv_dto(0.025)
def get_session_status(self) -> GetSessionStatusReturn:
self._send_cro(COMMAND_CODE.GET_S_STATUS)
resp = self._recv_dto(0.025)
info = resp[2] if resp[1] else None
return GetSessionStatusReturn(status=resp[0], info=info)
def build_checksum(self, size: int) -> bytes:
self._send_cro(COMMAND_CODE.BUILD_CHKSUM, struct.pack(f"{self.byte_order.value}I", size))
resp = self._recv_dto(30.0)
chksum_size = resp[0]
assert chksum_size <= 4, "checksum more than 4 bytes"
chksum = resp[1:1+chksum_size]
return chksum
def clear_memory(self, size: int) -> None:
self._send_cro(COMMAND_CODE.CLEAR_MEMORY, struct.pack(f"{self.byte_order.value}I", size))
self._recv_dto(30.0)
def program(self, size: int, data: bytes) -> int:
if size > 5:
raise ValueError("size must be less than 6")
if len(data) > 5:
raise ValueError("max data size is 5 bytes")
self._send_cro(COMMAND_CODE.PROGRAM, bytes([size]) + data)
resp = self._recv_dto(0.1)
# mta_addr_ext = resp[0]
mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
return mta_addr
def program_6_bytes(self, data: bytes) -> int:
if len(data) != 6:
raise ValueError("data size must be 6 bytes")
self._send_cro(COMMAND_CODE.PROGRAM_6, data)
resp = self._recv_dto(0.1)
# mta_addr_ext = resp[0]
mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
return mta_addr
def move_memory_block(self, size: int) -> None:
self._send_cro(COMMAND_CODE.MOVE, struct.pack(f"{self.byte_order.value}I", size))
self._recv_dto(0.025)
def diagnostic_service(self, service_num: int, data: bytes = b"") -> DiagnosticServiceReturn:
if service_num > 65535:
raise ValueError("service number must be less than 65536")
if len(data) > 4:
raise ValueError("max data size is 4 bytes")
self._send_cro(COMMAND_CODE.DIAG_SERVICE, struct.pack(f"{self.byte_order.value}H", service_num) + data)
resp = self._recv_dto(0.025)
return DiagnosticServiceReturn(length=resp[0], type=resp[1])
def action_service(self, service_num: int, data: bytes = b"") -> ActionServiceReturn:
if service_num > 65535:
raise ValueError("service number must be less than 65536")
if len(data) > 4:
raise ValueError("max data size is 4 bytes")
self._send_cro(COMMAND_CODE.ACTION_SERVICE, struct.pack(f"{self.byte_order.value}H", service_num) + data)
resp = self._recv_dto(0.025)
return ActionServiceReturn(length=resp[0], type=resp[1])
def test_availability(self, station_addr: int) -> None:
if station_addr > 65535:
raise ValueError("station address must be less than 65536")
# NOTE: station address is always little endian
self._send_cro(COMMAND_CODE.TEST, struct.pack("<H", station_addr))
self._recv_dto(0.025)
def start_stop_synchronised_transmission(self, mode: int) -> None:
if mode > 255:
raise ValueError("mode must be less than 256")
self._send_cro(COMMAND_CODE.START_STOP_ALL, bytes([mode]))
self._recv_dto(0.025)
def get_active_calibration_page(self):
self._send_cro(COMMAND_CODE.GET_ACTIVE_CAL_PAGE)
resp = self._recv_dto(0.025)
# cal_addr_ext = resp[0]
cal_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
return cal_addr
def get_version(self, desired_version: float = 2.1) -> float:
major, minor = map(int, str(desired_version).split("."))
self._send_cro(COMMAND_CODE.GET_CCP_VERSION, bytes([major, minor]))
resp = self._recv_dto(0.025)
return float(f"{resp[0]}.{resp[1]}")

0
artifacts/package_runtime/iqdbc/car/chrysler/__init__.py Normal file
View File

96
artifacts/package_runtime/iqdbc/car/chrysler/carcontroller.py Normal file
View File

@@ -0,0 +1,96 @@
from iqdbc.can import CANPacker
from iqdbc.car import Bus, DT_CTRL
from iqdbc.car.lateral import apply_meas_steer_torque_limits
from iqdbc.car.chrysler import chryslercan
from iqdbc.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags, RAM_DT
from iqdbc.car.interfaces import CarControllerBase
from iqdbc.lvbs.car.chrysler.iq_carcontroller import IQCarController
from iqdbc.lvbs.car.chrysler.aol import AolCarController
from iqdbc.lvbs.car.chrysler.iq_values import ChryslerFlagsIQ
class CarController(CarControllerBase, AolCarController, IQCarController):
def __init__(self, dbc_names, CP, CP_IQ):
CarControllerBase.__init__(self, dbc_names, CP, CP_IQ)
AolCarController.__init__(self)
IQCarController.__init__(self, CP, CP_IQ)
self.apply_torque_last = 0
self.hud_count = 0
self.last_lkas_falling_edge = 0
self.lkas_control_bit_prev = False
self.last_button_frame = 0
self.packer = CANPacker(dbc_names[Bus.pt])
self.params = CarControllerParams(CP)
def update(self, CC, CC_IQ, CS, now_nanos):
AolCarController.update(self, CC, CC_IQ, CS)
can_sends = []
lkas_active = CC.latActive and self.lkas_control_bit_prev
# cruise buttons
if (self.frame - self.last_button_frame) * DT_CTRL > 0.05:
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
# ACC cancellation
if CC.cruiseControl.cancel:
self.last_button_frame = self.frame
can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
# ACC resume from standstill
elif CC.cruiseControl.resume:
self.last_button_frame = self.frame
can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
# HUD alerts
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert,
self.hud_count, CS.lkas_car_model, CS.auto_high_beam, self.aol))
self.hud_count += 1
# steering
if self.frame % self.params.STEER_STEP == 0:
# TODO: can we make this more sane? why is it different for all the cars?
lkas_control_bit = self.lkas_control_bit_prev
if self.CP_IQ.flags & ChryslerFlagsIQ.NO_MIN_STEERING_SPEED or self.CP.carFingerprint in RAM_DT:
lkas_control_bit = IQCarController.get_lkas_control_bit(self, CS, CC, lkas_control_bit)
elif CS.out.vEgo > self.CP.minSteerSpeed:
lkas_control_bit = True
elif self.CP.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
lkas_control_bit = False
elif self.CP.carFingerprint in RAM_CARS:
if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5):
lkas_control_bit = False
# EPS faults if LKAS re-enables too quickly
lkas_control_bit = lkas_control_bit and (self.frame - self.last_lkas_falling_edge > 200)
if not lkas_control_bit and self.lkas_control_bit_prev:
self.last_lkas_falling_edge = self.frame
self.lkas_control_bit_prev = lkas_control_bit
# steer torque
new_torque = int(round(CC.actuators.torque * self.params.STEER_MAX))
apply_torque = apply_meas_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorqueEps, self.params)
if not lkas_active or not lkas_control_bit:
apply_torque = 0
self.apply_torque_last = apply_torque
can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_torque), lkas_control_bit))
if self.frame % 10 == 0 and self.CP.carFingerprint not in RAM_CARS:
can_sends.append(AolCarController.create_lkas_heartbit(self.packer, CS.lkas_heartbit, self.aol))
self.frame += 1
new_actuators = CC.actuators.as_builder()
new_actuators.torque = self.apply_torque_last / self.params.STEER_MAX
new_actuators.torqueOutputCan = self.apply_torque_last
return new_actuators, can_sends

123
artifacts/package_runtime/iqdbc/car/chrysler/carstate.py Normal file
View File

@@ -0,0 +1,123 @@
from iqdbc.can import CANDefine, CANParser
from iqdbc.car import Bus, create_button_events, structs
from iqdbc.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.interfaces import CarStateBase
from iqdbc.lvbs.car.chrysler.aol import AolCarState
from iqdbc.lvbs.car.chrysler.iq_carstate import IQCarState
ButtonType = structs.CarState.ButtonEvent.Type
class CarState(CarStateBase, AolCarState, IQCarState):
def __init__(self, CP, CP_IQ):
CarStateBase.__init__(self, CP, CP_IQ)
AolCarState.__init__(self, CP, CP_IQ)
IQCarState.__init__(self, CP, CP_IQ)
self.CP = CP
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.auto_high_beam = 0
self.button_counter = 0
self.lkas_car_model = -1
if CP.carFingerprint in RAM_CARS:
self.shifter_values = can_define.dv["Transmission_Status"]["Gear_State"]
else:
self.shifter_values = can_define.dv["GEAR"]["PRNDL"]
self.distance_button = 0
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
ret = structs.CarState()
ret_iq = structs.IQCarState()
prev_distance_button = self.distance_button
self.distance_button = cp.vl["CRUISE_BUTTONS"]["ACC_Distance_Dec"]
# lock info
ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"],
cp.vl["BCM_1"]["DOOR_OPEN_FR"],
cp.vl["BCM_1"]["DOOR_OPEN_RL"],
cp.vl["BCM_1"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = cp.vl["ORC_1"]["SEATBELT_DRIVER_UNLATCHED"] == 1
# brake pedal
ret.brake = 0
ret.brakePressed = cp.vl["ESP_1"]['Brake_Pedal_State'] == 1 # Physical brake pedal switch
# gas pedal
ret.gasPressed = cp.vl["ECM_5"]["Accelerator_Position"] > 1e-5
# car speed
if self.CP.carFingerprint in RAM_CARS:
ret.vEgoRaw = cp.vl["ESP_8"]["Vehicle_Speed"] * CV.KPH_TO_MS
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["Transmission_Status"]["Gear_State"], None))
else:
ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2.
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None))
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
# button presses
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1,
cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2)
ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1
# steering wheel
ret.steeringAngleDeg = cp.vl["STEERING"]["STEERING_ANGLE"] + cp.vl["STEERING"]["STEERING_ANGLE_HP"]
ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"]
ret.steeringTorque = cp.vl["EPS_2"]["COLUMN_TORQUE"]
ret.steeringTorqueEps = cp.vl["EPS_2"]["EPS_TORQUE_MOTOR"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# cruise state
cp_cruise = cp_cam if self.CP.carFingerprint in RAM_CARS else cp
ret.cruiseState.available = cp_cruise.vl["DAS_3"]["ACC_AVAILABLE"] == 1
ret.cruiseState.enabled = cp_cruise.vl["DAS_3"]["ACC_ACTIVE"] == 1
ret.cruiseState.speed = cp_cruise.vl["DAS_4"]["ACC_SET_SPEED_KPH"] * CV.KPH_TO_MS
ret.cruiseState.nonAdaptive = cp_cruise.vl["DAS_4"]["ACC_STATE"] in (1, 2) # 1 NormalCCOn and 2 NormalCCSet
ret.cruiseState.standstill = cp_cruise.vl["DAS_3"]["ACC_STANDSTILL"] == 1
ret.accFaulted = cp_cruise.vl["DAS_3"]["ACC_FAULTED"] != 0
if self.CP.carFingerprint in RAM_CARS:
# Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message
self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON']
ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1
else:
ret.steerFaultTemporary = cp.vl["EPS_2"]["LKAS_TEMPORARY_FAULT"] == 1
ret.steerFaultPermanent = cp.vl["EPS_2"]["LKAS_STATE"] == 4
# blindspot sensors
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["BSM_1"]["LEFT_STATUS"] == 1
ret.rightBlindspot = cp.vl["BSM_1"]["RIGHT_STATUS"] == 1
self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"]
self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"]
AolCarState.update_aol(self, ret, can_parsers)
IQCarState.update(self, ret, ret_iq, can_parsers)
ret.buttonEvents = [
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas}),
*self.button_events,
]
return ret, ret_iq
@staticmethod
def get_can_parsers(CP, CP_IQ):
pt_messages: list = []
cam_messages: list = []
AolCarState.get_parser(CP, pt_messages, cam_messages)
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
}

118
artifacts/package_runtime/iqdbc/car/chrysler/chryslercan.py Normal file
View File

@@ -0,0 +1,118 @@
from iqdbc.car import structs
from iqdbc.car.crc import CRC8J1850
from iqdbc.car.chrysler.values import RAM_CARS
GearShifter = structs.CarState.GearShifter
VisualAlert = structs.CarControl.HUDControl.VisualAlert
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam, aol):
# LKAS_HUD - Controls what lane-keeping icon is displayed
# == Color ==
# 0 hidden?
# 1 white
# 2 green
# 3 ldw
# == Lines ==
# 03 white Lines
# 04 grey lines
# 09 left lane close
# 0A right lane close
# 0B left Lane very close
# 0C right Lane very close
# 0D left cross cross
# 0E right lane cross
# == Alerts ==
# 7 Normal
# 6 lane departure place hands on wheel
if aol.enable_aol:
color = 2 if lkas_active else 1 if aol.paused else 0
else:
color = 2 if lkas_active else 1
lines = 3 if lkas_active else 0
alerts = 7 if lkas_active else 0
if hud_count < (1 * 4): # first 3 seconds, 4Hz
alerts = 1
if hud_alert in (VisualAlert.ldw, VisualAlert.steerRequired):
color = 4
lines = 0
alerts = 6
values = {
"LKAS_ICON_COLOR": color,
"CAR_MODEL": car_model,
"LKAS_LANE_LINES": lines,
"LKAS_ALERTS": alerts,
}
if CP.carFingerprint in RAM_CARS:
values['AUTO_HIGH_BEAM_ON'] = auto_high_beam
return packer.make_can_msg("DAS_6", 0, values)
def create_lkas_command(packer, CP, apply_torque, lkas_control_bit):
# LKAS_COMMAND Lane-keeping signal to turn the wheel
enabled_val = 2 if CP.carFingerprint in RAM_CARS else 1
values = {
"STEERING_TORQUE": apply_torque,
"LKAS_CONTROL_BIT": enabled_val if lkas_control_bit else 0,
}
return packer.make_can_msg("LKAS_COMMAND", 0, values)
def create_cruise_buttons(packer, frame, bus, cancel=False, resume=False, accel=False, decel=False):
values = {
"ACC_Cancel": cancel,
"ACC_Resume": resume,
"ACC_Accel": accel,
"ACC_Decel": decel,
"COUNTER": frame % 0x10,
}
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
def chrysler_checksum(address: int, sig, d: bytearray) -> int:
checksum = 0xFF
for j in range(len(d) - 1):
curr = d[j]
shift = 0x80
for _ in range(8):
bit_sum = curr & shift
temp_chk = checksum & 0x80
if bit_sum:
bit_sum = 0x1C
if temp_chk:
bit_sum = 1
checksum = (checksum << 1) & 0xFF
temp_chk = checksum | 1
bit_sum ^= temp_chk
else:
if temp_chk:
bit_sum = 0x1D
checksum = (checksum << 1) & 0xFF
bit_sum ^= checksum
checksum = bit_sum & 0xFF
shift >>= 1
return (~checksum) & 0xFF
def fca_giorgio_checksum(address: int, sig, d: bytearray) -> int:
crc = 0
for i in range(len(d) - 1):
crc ^= d[i]
crc = CRC8J1850[crc]
if address == 0xDE:
return crc ^ 0x10
elif address == 0x106:
return crc ^ 0xF6
elif address == 0x122:
return crc ^ 0xF1
else:
return crc ^ 0x0A

794
artifacts/package_runtime/iqdbc/car/chrysler/fingerprints.py Normal file
View File

@@ -0,0 +1,794 @@
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
from iqdbc.car.structs import CarParams
from iqdbc.car.chrysler.values import CAR
from iqdbc.lvbs.car.iq_fingerprints import extend_fw_versions
from iqdbc.lvbs.car.chrysler.iq_fingerprints import FW_VERSIONS_EXT
Ecu = CarParams.Ecu
FW_VERSIONS = {
CAR.CHRYSLER_PACIFICA_2018: {
(Ecu.combinationMeter, 0x742, None): [
b'68227902AF',
b'68227902AG',
b'68227902AH',
b'68227905AG',
b'68360252AC',
],
(Ecu.srs, 0x744, None): [
b'68211617AF',
b'68211617AG',
b'68358974AC',
b'68405937AA',
],
(Ecu.abs, 0x747, None): [
b'68222747AG',
b'68330876AA',
b'68330876AB',
b'68352227AA',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672758AA',
b'04672758AB',
b'68226356AF',
b'68226356AH',
b'68226356AI',
],
(Ecu.eps, 0x75a, None): [
b'68288891AE',
b'68378884AA',
b'68525338AA',
b'68525338AB',
],
(Ecu.engine, 0x7e0, None): [
b'68267018AO ',
b'68267020AJ ',
b'68303534AG ',
b'68303534AJ ',
b'68340762AD ',
b'68340764AD ',
b'68352652AE ',
b'68352654AE ',
b'68366851AH ',
b'68366853AE ',
b'68366853AG ',
b'68372861AF ',
],
(Ecu.transmission, 0x7e1, None): [
b'68277370AJ',
b'68277370AM',
b'68277372AD',
b'68277372AE',
b'68277372AN',
b'68277374AA',
b'68277374AB',
b'68277374AD',
b'68277374AN',
b'68367471AC',
b'68367471AD',
b'68380571AB',
],
},
CAR.CHRYSLER_PACIFICA_2020: {
(Ecu.combinationMeter, 0x742, None): [
b'68405327AC',
b'68436233AB',
b'68436233AC',
b'68436234AB',
b'68436250AE',
b'68529067AA',
b'68594993AB',
b'68594994AB',
],
(Ecu.srs, 0x744, None): [
b'68405565AB',
b'68405565AC',
b'68444299AC',
b'68480707AC',
b'68480708AC',
b'68526663AB',
],
(Ecu.abs, 0x747, None): [
b'68397394AA',
b'68433480AB',
b'68453575AF',
b'68577676AA',
b'68593395AA',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672758AA',
b'04672758AB',
b'68417813AF',
b'68540436AA',
b'68540436AB',
b'68540436AC',
b'68540436AD',
b'68598670AB',
b'68598670AC',
],
(Ecu.eps, 0x75a, None): [
b'68416742AA',
b'68460393AA',
b'68460393AB',
b'68494461AB',
b'68494461AC',
b'68524936AA',
b'68524936AB',
b'68525338AB',
b'68594337AB',
b'68594340AB',
],
(Ecu.engine, 0x7e0, None): [
b'68413871AD ',
b'68413871AE ',
b'68413871AH ',
b'68413871AI ',
b'68413871AJ ',
b'68413873AH ',
b'68413873AI ',
b'68443120AE ',
b'68443123AC ',
b'68443125AC ',
b'68496647AI ',
b'68496647AJ ',
b'68496650AH ',
b'68496650AI ',
b'68496650AL ',
b'68496652AH ',
b'68526752AD ',
b'68526752AE ',
b'68526754AD ',
b'68526754AE ',
b'68536264AE ',
b'68700304AB ',
b'68700306AB ',
],
(Ecu.transmission, 0x7e1, None): [
b'68414271AC',
b'68414271AD',
b'68414275AC',
b'68414275AD',
b'68443154AB',
b'68443154AC',
b'68443155AC',
b'68443158AB',
b'68501050AD',
b'68501051AD',
b'68501055AD',
b'68527221AB',
b'68527223AB',
b'68586231AD',
b'68586233AD',
],
},
CAR.CHRYSLER_PACIFICA_2018_HYBRID: {
(Ecu.combinationMeter, 0x742, None): [
b'68239262AH',
b'68239262AI',
b'68239262AJ',
b'68239263AH',
b'68239263AJ',
b'68358439AE',
b'68358439AG',
],
(Ecu.srs, 0x744, None): [
b'68238840AH',
b'68358990AC',
b'68405939AA',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672758AA',
b'68226356AI',
],
(Ecu.eps, 0x75a, None): [
b'68288309AC',
b'68288309AD',
b'68525339AA',
],
(Ecu.engine, 0x7e0, None): [
b'68277480AV ',
b'68277480AX ',
b'68277480AZ ',
b'68366580AI ',
b'68366580AK ',
b'68366580AM ',
],
(Ecu.hybrid, 0x7e2, None): [
b'05190175BF',
b'05190175BH',
b'05190226AI',
b'05190226AK',
b'05190226AM',
],
},
CAR.CHRYSLER_PACIFICA_2019_HYBRID: {
(Ecu.combinationMeter, 0x742, None): [
b'68405292AC',
b'68434956AC',
b'68434956AD',
b'68434960AE',
b'68434960AF',
b'68529064AB',
b'68594990AB',
b'68594990AD',
b'68594990AE',
b'68594991AB',
],
(Ecu.srs, 0x744, None): [
b'68405567AB',
b'68405567AC',
b'68453076AD',
b'68480710AC',
b'68526665AB',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672758AB',
b'68417813AF',
b'68540436AA',
b'68540436AB',
b'68540436AC',
b'68540436AD',
b'68598670AB',
b'68598670AC',
b'68645752AA',
],
(Ecu.eps, 0x75a, None): [
b'68416741AA',
b'68460392AA',
b'68525339AA',
b'68525339AB',
b'68594341AB',
b'68594341AC',
],
(Ecu.engine, 0x7e0, None): [
b'05190392AB ',
b'68416680AD ',
b'68416680AE ',
b'68416680AF ',
b'68416680AG ',
b'68444228AC ',
b'68444228AD ',
b'68444228AE ',
b'68444228AF ',
b'68499122AD ',
b'68499122AE ',
b'68499122AF ',
b'68526772AD ',
b'68526772AH ',
b'68599493AC ',
b'68657433AA ',
b'68700317AC ',
],
(Ecu.hybrid, 0x7e2, None): [
b'05185116AF',
b'05185116AJ',
b'05185116AK',
b'05185116AL',
b'05190240AP',
b'05190240AQ',
b'05190240AR',
b'05190265AG',
b'05190265AH',
b'05190289AE',
b'68540977AH',
b'68540977AK',
b'68540977AL',
b'68597647AE',
b'68597647AF',
b'68632416AB',
b'68632416AC',
b'68676877AB',
],
},
CAR.JEEP_GRAND_CHEROKEE: {
(Ecu.combinationMeter, 0x742, None): [
b'68243549AG',
b'68302211AC',
b'68302212AD',
b'68302214AC',
b'68302223AC',
b'68302246AC',
b'68331511AC',
b'68331574AC',
b'68331687AC',
b'68331690AC',
b'68340272AD',
],
(Ecu.srs, 0x744, None): [
b'68309533AA',
b'68316742AB',
b'68355363AB',
],
(Ecu.abs, 0x747, None): [
b'68252642AG',
b'68306178AD',
b'68336275AB',
b'68336276AB',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672627AB',
b'68251506AF',
b'68332015AB',
],
(Ecu.eps, 0x75a, None): [
b'68276201AG',
b'68321644AB',
b'68321644AC',
b'68321646AC',
b'68321648AC',
b'68321650AC',
],
(Ecu.engine, 0x7e0, None): [
b'05035920AE ',
b'68252272AG ',
b'68284455AI ',
b'68284456AI ',
b'68284456AJ ',
b'68284477AF ',
b'68325564AH ',
b'68325564AI ',
b'68325565AH ',
b'68325565AI ',
b'68325565AJ ',
b'68325618AD ',
],
(Ecu.transmission, 0x7e1, None): [
b'05035517AH',
b'68253222AF',
b'68311218AC',
b'68311218AD',
b'68311223AF',
b'68311223AG',
b'68361911AE',
b'68361911AF',
b'68361911AH',
b'68361914AE',
b'68361916AD',
],
},
CAR.JEEP_GRAND_CHEROKEE_2019: {
(Ecu.combinationMeter, 0x742, None): [
b'68402703AB',
b'68402704AB',
b'68402707AB',
b'68402708AB',
b'68402714AB',
b'68402736AB',
b'68402971AD',
b'68454144AD',
b'68454145AB',
b'68454152AB',
b'68454156AB',
b'68516650AB',
b'68516651AB',
b'68516669AB',
b'68516671AB',
b'68516683AB',
],
(Ecu.srs, 0x744, None): [
b'68355363AB',
b'68355364AB',
],
(Ecu.abs, 0x747, None): [
b'68408639AC',
b'68408639AD',
b'68499978AB',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672788AA',
b'68456722AC',
],
(Ecu.eps, 0x75a, None): [
b'68417279AA',
b'68417280AA',
b'68417281AA',
b'68417283AA',
b'68453431AA',
b'68453433AA',
b'68453435AA',
b'68499171AA',
b'68499171AB',
b'68501183AA',
b'68501186AA',
],
(Ecu.engine, 0x7e0, None): [
b'05035674AB ',
b'68412635AE ',
b'68412635AG ',
b'68412635AH ',
b'68412660AD ',
b'68412660AF ',
b'68422860AB',
b'68449435AE ',
b'68496223AA ',
b'68504959AD ',
b'68504959AE ',
b'68504960AD ',
b'68504993AC ',
],
(Ecu.transmission, 0x7e1, None): [
b'05035707AA',
b'68419672AC',
b'68419675AC',
b'68419678AB',
b'68423905AB',
b'68449258AC',
b'68495807AA',
b'68495807AB',
b'68503641AC',
b'68503644AC',
b'68503664AC',
],
},
CAR.RAM_1500_5TH_GEN: {
(Ecu.combinationMeter, 0x742, None): [
b'68294051AG',
b'68294051AI',
b'68294052AG',
b'68294052AH',
b'68294059AI',
b'68294063AG',
b'68294063AH',
b'68294063AI',
b'68434846AC',
b'68434847AC',
b'68434849AC',
b'68434850AC',
b'68434855AC',
b'68434856AC',
b'68434858AC',
b'68434859AC',
b'68434860AC',
b'68453471AD',
b'68453483AC',
b'68453483AD',
b'68453487AD',
b'68453491AC',
b'68453491AD',
b'68453499AD',
b'68453502AC',
b'68453503AC',
b'68453503AD',
b'68453505AC',
b'68453505AD',
b'68453511AC',
b'68453513AC',
b'68453513AD',
b'68453514AD',
b'68505633AB',
b'68510277AG',
b'68510277AH',
b'68510280AG',
b'68510280AH',
b'68510282AG',
b'68510282AH',
b'68510283AG',
b'68527346AE',
b'68527361AD',
b'68527375AD',
b'68527381AD',
b'68527381AE',
b'68527382AE',
b'68527383AD',
b'68527383AE',
b'68527387AE',
b'68527397AD',
b'68527403AC',
b'68527403AD',
b'68527404AD',
b'68546047AF',
b'68631938AA',
b'68631939AA',
b'68631940AA',
b'68631940AB',
b'68631941AB',
b'68631942AA',
b'68631943AB',
],
(Ecu.srs, 0x744, None): [
b'68428609AB',
b'68441329AA',
b'68441329AB',
b'68473844AB',
b'68490898AA',
b'68500728AA',
b'68615033AA',
b'68615034AA',
],
(Ecu.abs, 0x747, None): [
b'68292406AG',
b'68292406AH',
b'68432418AB',
b'68432418AC',
b'68432418AD',
b'68436004AD',
b'68436004AE',
b'68438454AC',
b'68438454AD',
b'68438456AE',
b'68438456AF',
b'68535469AB',
b'68535470AC',
b'68548900AB',
b'68548900AC',
b'68586307AB',
b'68586307AC',
b'68728724AA',
b'68728727AA',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672892AB',
b'04672932AB',
b'04672932AC',
b'22DTRHD_AA',
b'68320950AH',
b'68320950AI',
b'68320950AJ',
b'68320950AL',
b'68320950AM',
b'68454268AB',
b'68454268AC',
b'68475160AE',
b'68475160AF',
b'68475160AG',
],
(Ecu.eps, 0x75a, None): [
b'21590101AA',
b'21590101AB',
b'22490101AB',
b'68273275AF',
b'68273275AG',
b'68273275AH',
b'68312176AE',
b'68312176AF',
b'68312176AG',
b'68440789AC',
b'68466110AA',
b'68466110AB',
b'68466113AA',
b'68466116AA',
b'68469901AA',
b'68469904AA',
b'68469907AA',
b'68522583AA',
b'68522583AB',
b'68522584AA',
b'68522585AB',
b'68552788AA',
b'68552789AA',
b'68552790AA',
b'68552791AB',
b'68552794AA',
b'68552794AD',
b'68585106AB',
b'68585107AB',
b'68585108AB',
b'68585109AB',
b'68585112AB',
],
(Ecu.engine, 0x7e0, None): [
b'05035699AG ',
b'05035841AC ',
b'05035841AD ',
b'05036026AB ',
b'05036030AC ',
b'05036065AE ',
b'05036066AE ',
b'05036067AE ',
b'05036193AA ',
b'05149368AA ',
b'05149374AA ',
b'05149591AD ',
b'05149591AE ',
b'05149592AE ',
b'05149599AE ',
b'05149600AD ',
b'05149600AE ',
b'05149605AE ',
b'05149846AA ',
b'05149848AA ',
b'05149848AC ',
b'05190341AD',
b'05190346AD',
b'68378695AI ',
b'68378695AJ ',
b'68378695AK ',
b'68378696AJ ',
b'68378696AK ',
b'68378701AI ',
b'68378702AI ',
b'68378710AL ',
b'68378742AI ',
b'68378742AK ',
b'68378743AI ',
b'68378743AM ',
b'68378748AL ',
b'68378758AM ',
b'68378759AM ',
b'68448163AJ',
b'68448163AK',
b'68448163AL',
b'68448165AG',
b'68448165AK',
b'68455111AC ',
b'68455119AC ',
b'68455137AC ',
b'68455142AC ',
b'68455142AE ',
b'68455145AC ',
b'68455145AE ',
b'68455146AC ',
b'68460927AA ',
b'68467909AB ',
b'68467909AC ',
b'68467915AC ',
b'68467916AC ',
b'68467936AC ',
b'68500630AD',
b'68500630AE',
b'68500630AF',
b'68500631AE',
b'68502719AC ',
b'68502722AC ',
b'68502733AC ',
b'68502734AF ',
b'68502737AF ',
b'68502740AF ',
b'68502741AF ',
b'68502742AC ',
b'68502742AF ',
b'68539650AD',
b'68539650AF',
b'68539651AD',
b'68586101AA ',
b'68586102AA ',
b'68586105AB ',
b'68629917AC ',
b'68629919AC ',
b'68629919AD ',
b'68629922AC ',
b'68629925AC ',
b'68629926AC ',
],
(Ecu.transmission, 0x7e1, None): [
b'05035706AD',
b'05035842AB',
b'05036069AA',
b'05036181AA',
b'05149536AC',
b'05149537AC',
b'05149543AC',
b'68360078AL',
b'68360080AL',
b'68360080AM',
b'68360081AM',
b'68360081AN',
b'68360085AH',
b'68360085AJ',
b'68360085AK',
b'68360085AL',
b'68360085AO',
b'68360086AH',
b'68360086AK',
b'68360086AN',
b'68384328AD',
b'68384332AD',
b'68445531AC',
b'68445532AB',
b'68445533AB',
b'68445536AB',
b'68445537AB',
b'68466081AB',
b'68466086AB',
b'68466087AB',
b'68484466AC',
b'68484467AC',
b'68484471AC',
b'68502994AC',
b'68502994AD',
b'68502996AD',
b'68520867AE',
b'68520867AF',
b'68520870AC',
b'68520871AC',
b'68528325AE',
b'68540431AB',
b'68540433AB',
b'68551676AA',
b'68629935AB',
b'68629936AC',
],
},
CAR.RAM_HD_5TH_GEN: {
(Ecu.combinationMeter, 0x742, None): [
b'68361606AH',
b'68437735AC',
b'68492693AD',
b'68525485AB',
b'68525487AB',
b'68525498AB',
b'68528791AF',
b'68628474AB',
],
(Ecu.srs, 0x744, None): [
b'68399794AC',
b'68428503AA',
b'68428505AA',
b'68428507AA',
],
(Ecu.abs, 0x747, None): [
b'68334977AH',
b'68455481AC',
b'68504022AA',
b'68504022AB',
b'68504022AC',
b'68530686AB',
b'68530686AC',
b'68544596AC',
b'68641704AA',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672895AB',
b'04672934AB',
b'56029827AG',
b'56029827AH',
b'68462657AE',
b'68484694AD',
b'68484694AE',
b'68615489AB',
],
(Ecu.eps, 0x761, None): [
b'68421036AC',
b'68507906AB',
b'68534023AC',
],
(Ecu.engine, 0x7e0, None): [
b'52370131AF',
b'52370231AF',
b'52370231AG',
b'52370491AA',
b'52370931CT',
b'52401032AE',
b'52421132AF',
b'52421332AF',
b'68527616AD ',
b'M2370131MB',
b'M2421132MB',
],
},
CAR.DODGE_DURANGO: {
(Ecu.combinationMeter, 0x742, None): [
b'68454261AD',
b'68471535AE',
],
(Ecu.srs, 0x744, None): [
b'68355362AB',
b'68492238AD',
],
(Ecu.abs, 0x747, None): [
b'68408639AD',
b'68499978AB',
],
(Ecu.fwdRadar, 0x753, None): [
b'68440581AE',
b'68456722AC',
],
(Ecu.eps, 0x75a, None): [
b'68453435AA',
b'68498477AA',
],
(Ecu.engine, 0x7e0, None): [
b'05035786AE ',
b'68449476AE ',
],
(Ecu.transmission, 0x7e1, None): [
b'05035826AC',
b'68449265AC',
],
},
}
FW_VERSIONS = extend_fw_versions(FW_VERSIONS, FW_VERSIONS_EXT)

109
artifacts/package_runtime/iqdbc/car/chrysler/interface.py Normal file
View File

@@ -0,0 +1,109 @@
#!/usr/bin/env python3
from iqdbc.car import get_safety_config, structs
from iqdbc.car.chrysler.carcontroller import CarController
from iqdbc.car.chrysler.carstate import CarState
from iqdbc.car.chrysler.radar_interface import RadarInterface
from iqdbc.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags, ChryslerSafetyFlags
from iqdbc.car.interfaces import CarInterfaceBase
from iqdbc.lvbs.car.chrysler.iq_values import ChryslerFlagsIQ
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
DRIVABLE_GEARS = (structs.CarState.GearShifter.low,)
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "chrysler"
ret.dashcamOnly = candidate in RAM_HD
# radar parsing needs some work, see https://github.com/commaai/openpilot/issues/26842
ret.radarUnavailable = True # Bus.radar not in DBC[candidate][Bus.radar]
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.4
# safety config
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.chrysler)]
if candidate in RAM_HD:
ret.safetyConfigs[0].safetyParam |= ChryslerSafetyFlags.RAM_HD.value
elif candidate in RAM_DT:
ret.safetyConfigs[0].safetyParam |= ChryslerSafetyFlags.RAM_DT.value
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate not in RAM_CARS:
# Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed.
new_eps_platform = candidate in (CAR.CHRYSLER_PACIFICA_2019_HYBRID, CAR.CHRYSLER_PACIFICA_2020, CAR.JEEP_GRAND_CHEROKEE_2019, CAR.DODGE_DURANGO)
new_eps_firmware = any(fw.ecu == 'eps' and fw.fwVersion[:4] >= b"6841" for fw in car_fw)
if new_eps_platform or new_eps_firmware:
ret.flags |= ChryslerFlags.HIGHER_MIN_STEERING_SPEED.value
# Chrysler
if candidate in (CAR.CHRYSLER_PACIFICA_2018, CAR.CHRYSLER_PACIFICA_2018_HYBRID, CAR.CHRYSLER_PACIFICA_2019_HYBRID,
CAR.CHRYSLER_PACIFICA_2020, CAR.DODGE_DURANGO):
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]]
ret.lateralTuning.pid.kf = 0.00006
# Jeep
elif candidate in (CAR.JEEP_GRAND_CHEROKEE, CAR.JEEP_GRAND_CHEROKEE_2019):
ret.steerActuatorDelay = 0.2
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]]
ret.lateralTuning.pid.kf = 0.00006
# Ram
elif candidate == CAR.RAM_1500_5TH_GEN:
ret.steerActuatorDelay = 0.2
ret.wheelbase = 3.88
# Older EPS FW allow steer to zero
if any(fw.ecu == 'eps' and b"68" < fw.fwVersion[:4] <= b"6831" for fw in car_fw):
ret.minSteerSpeed = 0.
elif candidate == CAR.RAM_HD_5TH_GEN:
ret.steerActuatorDelay = 0.2
else:
raise ValueError(f"Unsupported car: {candidate}")
if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
# TODO: allow these cars to steer down to 13 m/s if already engaged.
# TODO: Durango 2020 may be able to steer to zero once above 38 kph
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged.
ret.centerToFront = ret.wheelbase * 0.44
ret.enableBsm = 720 in fingerprint[0]
return ret
@staticmethod
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]],
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
if candidate == CAR.RAM_1500_5TH_GEN:
if stock_cp.minSteerSpeed != 0.:
stock_cp.minSteerSpeed = 0.5
stock_cp.minEnableSpeed = 14.5
if any(fw.ecu == 'eps' and fw.fwVersion in (b"68273275AF", b"68273275AG", b"68312176AE", b"68312176AG",) for fw in car_fw):
stock_cp.minEnableSpeed = 0.
if candidate == CAR.RAM_HD_5TH_GEN:
stock_cp.dashcamOnly = False
# https://github.com/commaai/openpilot/issues/25389
stock_cp.tireStiffnessFactor = 1.0
stock_cp.tireStiffnessFront = 65155.
stock_cp.tireStiffnessRear = 80926.
stock_cp.wheelbase = 3.79
stock_cp.steerRatio = 19.
# LKAS heartbeat on bus 0 (msg 0x4FF) means the camera is on the ADAS bus and
# IQ.Pilot can steer down to a standstill.
if 0x4FF in fingerprint[0]:
ret.flags |= ChryslerFlagsIQ.NO_MIN_STEERING_SPEED.value
stock_cp.minSteerSpeed = 0.
return ret

85
artifacts/package_runtime/iqdbc/car/chrysler/radar_interface.py Normal file
View File

@@ -0,0 +1,85 @@
#!/usr/bin/env python3
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.interfaces import RadarInterfaceBase
from iqdbc.car.chrysler.values import DBC
RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724
RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages
LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D)
NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D)
def _create_radar_can_parser(car_fingerprint):
if Bus.radar not in DBC[car_fingerprint]:
return None
msg_n = len(RADAR_MSGS_C)
# list of [(signal name, message name or number), (...)]
# [('RADAR_STATE', 1024),
# ('LONG_DIST', 1072),
# ('LONG_DIST', 1073),
# ('LONG_DIST', 1074),
# ('LONG_DIST', 1075),
messages = list(zip(RADAR_MSGS_C +
RADAR_MSGS_D,
[20] * msg_n + # 20Hz (0.05s)
[20] * msg_n, strict=True)) # 20Hz (0.05s)
return CANParser(DBC[car_fingerprint][Bus.radar], messages, 1)
def _address_to_track(address):
if address in RADAR_MSGS_C:
return (address - RADAR_MSGS_C[0]) // 2
if address in RADAR_MSGS_D:
return (address - RADAR_MSGS_D[0]) // 2
raise ValueError("radar received unexpected address %d" % address)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP, CP_IQ):
super().__init__(CP, CP_IQ)
self.rcp = _create_radar_can_parser(CP.carFingerprint)
self.updated_messages = set()
self.trigger_msg = LAST_MSG
def update(self, can_strings):
if self.rcp is None or self.CP.radarUnavailable:
return super().update(None)
vls = self.rcp.update(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
ret = structs.RadarData()
if not self.rcp.can_valid:
ret.errors.canError = True
for ii in self.updated_messages: # ii should be the message ID as a number
cpt = self.rcp.vl[ii]
trackId = _address_to_track(ii)
if trackId not in self.pts:
self.pts[trackId] = structs.RadarData.RadarPoint()
self.pts[trackId].trackId = trackId
self.pts[trackId].aRel = float('nan')
self.pts[trackId].yvRel = float('nan')
self.pts[trackId].measured = True
if 'LONG_DIST' in cpt: # c_* message
self.pts[trackId].dRel = cpt['LONG_DIST'] # from front of car
# our lat_dist is positive to the right in car's frame.
# LAT_DIST is right-positive, yRel is left-positive
self.pts[trackId].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
else: # d_* message
self.pts[trackId].vRel = cpt['REL_SPEED']
# We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid.
ret.points = [x for x in self.pts.values() if x.dRel != 0]
self.updated_messages.clear()
return ret

160
artifacts/package_runtime/iqdbc/car/chrysler/values.py Normal file
View File

@@ -0,0 +1,160 @@
from enum import IntFlag
from dataclasses import dataclass, field
from iqdbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from iqdbc.car.structs import CarParams
from iqdbc.car.docs_definitions import CarHarness, CarDocs, CarParts
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = CarParams.Ecu
class ChryslerSafetyFlags(IntFlag):
RAM_DT = 1
RAM_HD = 2
class ChryslerFlags(IntFlag):
# Detected flags
HIGHER_MIN_STEERING_SPEED = 1
@dataclass
class ChryslerCarDocs(CarDocs):
package: str = "Adaptive Cruise Control (ACC)"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.fca]))
@dataclass
class ChryslerPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {
Bus.pt: 'chrysler_pacifica_2017_hybrid_generated',
Bus.radar: 'chrysler_pacifica_2017_hybrid_private_fusion',
})
@dataclass(frozen=True)
class ChryslerCarSpecs(CarSpecs):
minSteerSpeed: float = 3.8 # m/s
class CAR(Platforms):
# Chrysler
CHRYSLER_PACIFICA_2018_HYBRID = ChryslerPlatformConfig(
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2017-18")],
ChryslerCarSpecs(mass=2242., wheelbase=3.089, steerRatio=16.2),
)
CHRYSLER_PACIFICA_2019_HYBRID = ChryslerPlatformConfig(
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2019-25")],
CHRYSLER_PACIFICA_2018_HYBRID.specs,
)
CHRYSLER_PACIFICA_2018 = ChryslerPlatformConfig(
[ChryslerCarDocs("Chrysler Pacifica 2017-18")],
CHRYSLER_PACIFICA_2018_HYBRID.specs,
)
CHRYSLER_PACIFICA_2020 = ChryslerPlatformConfig(
[
ChryslerCarDocs("Chrysler Pacifica 2019-20"),
ChryslerCarDocs("Chrysler Pacifica 2021-23", package="All"),
],
CHRYSLER_PACIFICA_2018_HYBRID.specs,
)
# Dodge
DODGE_DURANGO = ChryslerPlatformConfig(
[ChryslerCarDocs("Dodge Durango 2020-21")],
CHRYSLER_PACIFICA_2018_HYBRID.specs,
)
# Jeep
JEEP_GRAND_CHEROKEE = ChryslerPlatformConfig( # includes 2017 Trailhawk
[ChryslerCarDocs("Jeep Grand Cherokee 2016-18", video="https://www.youtube.com/watch?v=eLR9o2JkuRk")],
ChryslerCarSpecs(mass=1778., wheelbase=2.71, steerRatio=16.7),
)
JEEP_GRAND_CHEROKEE_2019 = ChryslerPlatformConfig( # includes 2020 Trailhawk
[ChryslerCarDocs("Jeep Grand Cherokee 2019-21", video="https://www.youtube.com/watch?v=jBe4lWnRSu4")],
JEEP_GRAND_CHEROKEE.specs,
)
# Ram
RAM_1500_5TH_GEN = ChryslerPlatformConfig(
[ChryslerCarDocs("Ram 1500 2019-24", car_parts=CarParts.common([CarHarness.ram]))],
ChryslerCarSpecs(mass=2493., wheelbase=3.88, steerRatio=16.3, minSteerSpeed=14.5),
{Bus.pt: 'chrysler_ram_dt_generated'},
)
RAM_HD_5TH_GEN = ChryslerPlatformConfig(
[
ChryslerCarDocs("Ram 2500 2020-24", car_parts=CarParts.common([CarHarness.ram])),
ChryslerCarDocs("Ram 3500 2019-22", car_parts=CarParts.common([CarHarness.ram])),
],
ChryslerCarSpecs(mass=3405., wheelbase=3.785, steerRatio=15.61, minSteerSpeed=16.),
{Bus.pt: 'chrysler_ram_hd_generated'},
)
class CarControllerParams:
def __init__(self, CP):
self.STEER_STEP = 2 # 50 Hz
self.STEER_ERROR_MAX = 80
if CP.carFingerprint in RAM_HD:
self.STEER_DELTA_UP = 14
self.STEER_DELTA_DOWN = 14
self.STEER_MAX = 361 # higher than this faults the EPS
elif CP.carFingerprint in RAM_DT:
self.STEER_DELTA_UP = 6
self.STEER_DELTA_DOWN = 6
self.STEER_MAX = 350 # EPS allows more, up to 350?
else:
self.STEER_DELTA_UP = 3
self.STEER_DELTA_DOWN = 3
self.STEER_MAX = 261 # higher than this faults the EPS
STEER_THRESHOLD = 120
RAM_DT = {CAR.RAM_1500_5TH_GEN, }
RAM_HD = {CAR.RAM_HD_5TH_GEN, }
RAM_CARS = RAM_DT | RAM_HD
CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf132)
CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(0xf132)
CHRYSLER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER)
CHRYSLER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER)
CHRYSLER_RX_OFFSET = -0x280
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[CHRYSLER_VERSION_REQUEST],
[CHRYSLER_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.srs, Ecu.fwdRadar, Ecu.combinationMeter],
rx_offset=CHRYSLER_RX_OFFSET,
bus=0,
),
Request(
[CHRYSLER_VERSION_REQUEST],
[CHRYSLER_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.hybrid, Ecu.engine, Ecu.transmission],
bus=0,
),
Request(
[CHRYSLER_SOFTWARE_VERSION_REQUEST],
[CHRYSLER_SOFTWARE_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.transmission],
bus=0,
),
],
extra_ecus=[
(Ecu.abs, 0x7e4, None), # alt address for abs on hybrids, NOTE: not on all hybrid platforms
],
)
DBC = CAR.create_dbc_map()

0
artifacts/package_runtime/iqdbc/car/common/__init__.py Normal file
View File

4
artifacts/package_runtime/iqdbc/car/common/basedir.py Normal file
View File

@@ -0,0 +1,4 @@
import os
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))

20
artifacts/package_runtime/iqdbc/car/common/conversions.py Normal file
View File

@@ -0,0 +1,20 @@
import numpy as np
class Conversions:
# Speed
MPH_TO_KPH = 1.609344
KPH_TO_MPH = 1. / MPH_TO_KPH
MS_TO_KPH = 3.6
KPH_TO_MS = 1. / MS_TO_KPH
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
MS_TO_KNOTS = 1.9438
KNOTS_TO_MS = 1. / MS_TO_KNOTS
# Angle
DEG_TO_RAD = np.pi / 180.
RAD_TO_DEG = 1. / DEG_TO_RAD
# Mass
LB_TO_KG = 0.453592

44
artifacts/package_runtime/iqdbc/car/common/filter_simple.py Normal file
View File

@@ -0,0 +1,44 @@
class FirstOrderFilter:
# first order filter
def __init__(self, x0, rc, dt, initialized=True):
self.x = x0
self._dt = dt
self.update_alpha(rc)
self.initialized = initialized
def update_dt(self, dt):
self._dt = dt
self.update_alpha(self._rc)
def update_alpha(self, rc):
self._rc = rc
self._alpha = self._dt / (self._rc + self._dt)
def update(self, x):
if self.initialized:
self.x = (1. - self._alpha) * self.x + self._alpha * x
else:
self.initialized = True
self.x = x
return self.x
class HighPassFilter:
# technically a band-pass filter
def __init__(self, x0, rc1, rc2, dt, initialized=True):
self.x = x0
self._f1 = FirstOrderFilter(x0, rc1, dt, initialized)
self._f2 = FirstOrderFilter(x0, rc2, dt, initialized)
assert rc2 > rc1, "rc2 must be greater than rc1"
def update_dt(self, dt):
self._f1.update_dt(dt)
self._f2.update_dt(dt)
def update_alpha(self, rc1, rc2):
self._f1.update_alpha(rc1)
self._f2.update_alpha(rc2)
def update(self, x):
self.x = self._f1.update(x) - self._f2.update(x)
return self.x

20
artifacts/package_runtime/iqdbc/car/common/numpy_fast.py Normal file
View File

@@ -0,0 +1,20 @@
def clip(x, lo, hi):
return max(lo, min(hi, x))
def interp(x, xp, fp):
N = len(xp)
def get_interp(xv):
hi = 0
while hi < N and xv > xp[hi]:
hi += 1
low = hi - 1
return fp[-1] if hi == N and xv > xp[low] else (
fp[0] if hi == 0 else
(xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low])
return [get_interp(v) for v in x] if hasattr(x, '__iter__') else get_interp(x)
def mean(x):
return sum(x) / len(x)

71
artifacts/package_runtime/iqdbc/car/common/pid.py Normal file
View File

@@ -0,0 +1,71 @@
import numpy as np
from numbers import Number
class PIDController:
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
self._k_p = k_p
self._k_i = k_i
self._k_d = k_d
self.k_f = k_f # feedforward gain
if isinstance(self._k_p, Number):
self._k_p = [[0], [self._k_p]]
if isinstance(self._k_i, Number):
self._k_i = [[0], [self._k_i]]
if isinstance(self._k_d, Number):
self._k_d = [[0], [self._k_d]]
self.pos_limit = pos_limit
self.neg_limit = neg_limit
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.speed = 0.0
self.reset()
@property
def k_p(self):
return np.interp(self.speed, self._k_p[0], self._k_p[1])
@property
def k_i(self):
return np.interp(self.speed, self._k_i[0], self._k_i[1])
@property
def k_d(self):
return np.interp(self.speed, self._k_d[0], self._k_d[1])
@property
def error_integral(self):
return self.i/self.k_i
def reset(self):
self.p = 0.0
self.i = 0.0
self.d = 0.0
self.f = 0.0
self.control = 0
def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
self.speed = speed
self.p = float(error) * self.k_p
self.f = feedforward * self.k_f
self.d = error_rate * self.k_d
if override:
self.i -= self.i_unwind_rate * float(np.sign(self.i))
else:
if not freeze_integrator:
self.i = self.i + error * self.k_i * self.i_rate
# Clip i to prevent exceeding control limits
control_no_i = self.p + self.d + self.f
control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
control = self.p + self.i + self.d + self.f
self.control = np.clip(control, self.neg_limit, self.pos_limit)
return self.control

54
artifacts/package_runtime/iqdbc/car/common/simple_kalman.py Normal file
View File

@@ -0,0 +1,54 @@
import numpy as np
def get_kalman_gain(dt, A, C, Q, R, iterations=100):
P = np.zeros_like(Q)
for _ in range(iterations):
P = A.dot(P).dot(A.T) + dt * Q
S = C.dot(P).dot(C.T) + R
K = P.dot(C.T).dot(np.linalg.inv(S))
P = (np.eye(len(P)) - K.dot(C)).dot(P)
return K
class KF1D:
# this EKF assumes constant covariance matrix, so calculations are much simpler
# the Kalman gain also needs to be precomputed using the control module
def __init__(self, x0, A, C, K):
self.x0_0 = x0[0][0]
self.x1_0 = x0[1][0]
self.A0_0 = A[0][0]
self.A0_1 = A[0][1]
self.A1_0 = A[1][0]
self.A1_1 = A[1][1]
self.C0_0 = C[0]
self.C0_1 = C[1]
self.K0_0 = K[0][0]
self.K1_0 = K[1][0]
self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
# K matrix needs to be pre-computed as follow:
# import control
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
# self.K = np.transpose(K)
def update(self, meas):
#self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
self.x0_0 = x0_0
self.x1_0 = x1_0
return [self.x0_0, self.x1_0]
@property
def x(self):
return [[self.x0_0], [self.x1_0]]
def set_x(self, x):
self.x0_0 = x[0][0]
self.x1_0 = x[1][0]

31
artifacts/package_runtime/iqdbc/car/crc.py Normal file
View File

@@ -0,0 +1,31 @@
def _gen_crc8_table(poly: int) -> list[int]:
table = []
for i in range(256):
crc = i
for _ in range(8):
if crc & 0x80:
crc = ((crc << 1) ^ poly) & 0xFF
else:
crc = (crc << 1) & 0xFF
table.append(crc)
return table
def _gen_crc16_table(poly: int) -> list[int]:
table = []
for i in range(256):
crc = i << 8
for _ in range(8):
if crc & 0x8000:
crc = ((crc << 1) ^ poly) & 0xFFFF
else:
crc = (crc << 1) & 0xFFFF
table.append(crc)
return table
CRC8H2F = _gen_crc8_table(0x2F)
CRC8J1850 = _gen_crc8_table(0x1D)
CRC8BODY = _gen_crc8_table(0xD5)
CRC16_XMODEM = _gen_crc16_table(0x1021)

82
artifacts/package_runtime/iqdbc/car/debug/format_fingerprints.py Normal file
View File

@@ -0,0 +1,82 @@
#!/usr/bin/env python3
import jinja2
import os
from iqdbc.car.common.basedir import BASEDIR
from iqdbc.car.interfaces import get_interface_attr
from iqdbc.car.structs import CarParams
Ecu = CarParams.Ecu
CARS = get_interface_attr('CAR')
FW_VERSIONS = get_interface_attr('FW_VERSIONS')
FINGERPRINTS = get_interface_attr('FINGERPRINTS')
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()}
FINGERPRINTS_PY_TEMPLATE = jinja2.Template("""
{%- if FINGERPRINTS[brand] and brand != 'body' %}
# ruff: noqa: E501
{% endif %}
\"\"\" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE.\"\"\"
{% if FW_VERSIONS[brand] %}
from iqdbc.car.structs import CarParams
{% endif %}
from iqdbc.car.{{brand}}.values import CAR
{% if FW_VERSIONS[brand] %}
Ecu = CarParams.Ecu
{% endif %}
{% if comments +%}
{{ comments | join() }}
{% endif %}
{% if FINGERPRINTS[brand] %}
FINGERPRINTS = {
{% for car, fingerprints in FINGERPRINTS[brand].items() %}
CAR.{{car.name}}: [{
{% for fingerprint in fingerprints %}
{% if not loop.first %}
{{ "{" }}
{% endif %}
{% for key, value in fingerprint.items() %}{{key}}: {{value}}{% if not loop.last %}, {% endif %}{% endfor %}
}{% if loop.last %}]{% endif %},
{% endfor %}
{% endfor %}
}
{% endif %}
FW_VERSIONS{% if not FW_VERSIONS[brand] %}: dict[str, dict[tuple, list[bytes]]]{% endif %} = {
{% for car, _ in FW_VERSIONS[brand].items() %}
CAR.{{car.name}}: {
{% for key, fw_versions in FW_VERSIONS[brand][car].items() %}
(Ecu.{{ECU_NAME[key[0]]}}, 0x{{"%0x" | format(key[1] | int)}}, \
{% if key[2] %}0x{{"%0x" | format(key[2] | int)}}{% else %}{{key[2]}}{% endif %}): [
{% for fw_version in (fw_versions + extra_fw_versions.get(car, {}).get(key, [])) | unique | sort %}
{{fw_version}},
{% endfor %}
],
{% endfor %}
},
{% endfor %}
}
""", trim_blocks=True)
def format_brand_fw_versions(brand, extra_fw_versions: None | dict[str, dict[tuple, list[bytes]]] = None):
extra_fw_versions = extra_fw_versions or {}
fingerprints_file = os.path.join(BASEDIR, f"{brand}/fingerprints.py")
with open(fingerprints_file) as f:
comments = [line for line in f.readlines() if line.startswith("#") and "noqa" not in line]
with open(fingerprints_file, "w") as f:
f.write(FINGERPRINTS_PY_TEMPLATE.render(brand=brand, comments=comments, ECU_NAME=ECU_NAME,
FINGERPRINTS=FINGERPRINTS, FW_VERSIONS=FW_VERSIONS,
extra_fw_versions=extra_fw_versions))
if __name__ == "__main__":
for brand in FW_VERSIONS.keys():
format_brand_fw_versions(brand)

80
artifacts/package_runtime/iqdbc/car/disable_ecu.py Normal file
View File

@@ -0,0 +1,80 @@
from iqdbc.car.can_definitions import CanData
from iqdbc.car.carlog import carlog
from iqdbc.car.isotp_parallel_query import IsoTpParallelQuery
EXT_DIAG_REQUEST = b'\x10\x03'
EXT_DIAG_RESPONSE = b'\x50\x03'
COM_CONT_RESPONSE = b''
CLEAR_DTC_REQUEST = b'\x14\xff\xff\xff'
CLEAR_DTC_RESPONSE = b'\x54'
FUNCTIONAL_ADDR_29BIT = 0x18DB33F1
CLEAR_DTC_ISOTP_SF = bytes([len(CLEAR_DTC_REQUEST)]) + CLEAR_DTC_REQUEST + b'\x00' * (7 - len(CLEAR_DTC_REQUEST))
def clear_all_dtcs(can_send, buses, functional_addr=FUNCTIONAL_ADDR_29BIT):
# broadcast clears stored DTCs on every ECU on the bus, including safety-relevant modules
for bus in buses:
carlog.warning(f"clear all DTCs (functional) on bus {bus} ...")
can_send([CanData(functional_addr, CLEAR_DTC_ISOTP_SF, bus)])
def clear_ecu_dtcs(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, timeout=0.1, retry=10, response_offset: int = 0x8):
carlog.warning(f"ecu clear DTCs {hex(addr), sub_addr} ...")
for i in range(retry):
try:
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], response_offset)
for _, _ in query.get_data(timeout).items():
carlog.warning("clear diagnostic information ...")
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [CLEAR_DTC_REQUEST], [CLEAR_DTC_RESPONSE], response_offset)
query.get_data(timeout)
carlog.warning("ecu DTCs cleared")
return True
except Exception:
carlog.exception("ecu clear DTCs exception")
carlog.error(f"ecu clear DTCs retry ({i + 1}) ...")
carlog.error("ecu clear DTCs failed")
return False
def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01',
timeout=0.1, retry=10, response_offset: int = 0x8, clear_dtc=False):
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
The ECU will stay silent as long as openpilot keeps sending Tester Present.
This is used to disable the radar in some cars. Openpilot will emulate the radar.
WARNING: THIS DISABLES AEB!"""
carlog.warning(f"ecu disable {hex(addr), sub_addr} ...")
for i in range(retry):
try:
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], response_offset)
for _, _ in query.get_data(timeout).items():
# a DTC clear can take the ECU several hundred ms, so it must complete before comms go down
if clear_dtc:
carlog.warning("clear diagnostic information ...")
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [CLEAR_DTC_REQUEST], [CLEAR_DTC_RESPONSE], response_offset)
query.get_data(timeout)
carlog.warning("communication control disable tx/rx ...")
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], response_offset)
query.get_data(0)
carlog.warning("ecu disabled")
return True
except Exception:
carlog.exception("ecu disable exception")
carlog.error(f"ecu disable retry ({i + 1}) ...")
carlog.error("ecu disable failed")
return False

113
artifacts/package_runtime/iqdbc/car/docs.py Normal file
View File

@@ -0,0 +1,113 @@
#!/usr/bin/env python3
import re
import os
import jinja2
import argparse
import unicodedata
from typing import get_args
from enum import Enum
from collections import defaultdict
from iqdbc.car.common.basedir import BASEDIR
from iqdbc.car import gen_empty_fingerprint
from iqdbc.car.structs import CarParams, IQCarParams
from iqdbc.car.docs_definitions import BaseCarHarness, CarDocs, Device, ExtraCarDocs, Column, ExtraCarsColumn, CommonFootnote, PartType, SupportType
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.interfaces import get_interface_attr
from iqdbc.car.values import Platform
from iqdbc.car.mock.values import CAR as MOCK
from iqdbc.car.extra_cars import CAR as EXTRA
EXTRA_CARS_MD_OUT = os.path.join(BASEDIR, "../", "../", "docs", "CARS.md")
# TODO: merge these platforms into normal car ports with SupportType flag
ExtraPlatform = Platform | EXTRA
EXTRA_BRANDS = get_args(ExtraPlatform)
EXTRA_PLATFORMS: dict[str, ExtraPlatform] = {str(platform): platform for brand in EXTRA_BRANDS for platform in brand}
def get_params_for_docs(platform) -> tuple[CarParams, IQCarParams]:
cp_platform = platform if platform in interfaces else MOCK.MOCK
CP: CarParams = interfaces[cp_platform].get_params(cp_platform, fingerprint=gen_empty_fingerprint(),
car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)],
alpha_long=True, is_release=True, docs=True)
CP_IQ: IQCarParams = interfaces[cp_platform].get_params_iq(CP, cp_platform, fingerprint=gen_empty_fingerprint(),
car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)],
alpha_long=True, is_release_iq=True, docs=True)
return CP, CP_IQ
def get_all_footnotes() -> dict[Enum, int]:
all_footnotes = list(CommonFootnote)
for footnotes in get_interface_attr("Footnote", ignore_none=True).values():
all_footnotes.extend(footnotes)
return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)}
def _natural_sort_key(s):
# NFKD normalization ensures accented characters sort with their base letter (e.g., Š sorts with S)
normalized = unicodedata.normalize('NFKD', s)
return [int(t) if t.isdigit() else t.lower() for t in re.split(r'(\d+)', normalized) if t]
def build_sorted_car_docs_list(platforms, footnotes=None):
collected_car_docs: list[CarDocs | ExtraCarDocs] = []
for platform in platforms.values():
car_docs = platform.config.car_docs
CP, CP_IQ = get_params_for_docs(platform)
if not len(car_docs):
continue
# A platform can include multiple car models
for _car_docs in car_docs:
if not hasattr(_car_docs, "row"):
_car_docs.init_make(CP)
_car_docs.init(CP, footnotes)
collected_car_docs.append(_car_docs)
# Sort cars by make and model + year
sorted_cars = sorted(collected_car_docs, key=lambda car: _natural_sort_key(car.name))
return sorted_cars
# CAUTION: This function is imported by shop.comma.ai and comma.ai/vehicles, test changes carefully
def get_all_car_docs() -> list[CarDocs]:
collected_footnotes = get_all_footnotes()
sorted_list: list[CarDocs] = build_sorted_car_docs_list(EXTRA_PLATFORMS, footnotes=collected_footnotes)
return sorted_list
def group_by_make(all_car_docs: list[CarDocs]) -> dict[str, list[CarDocs]]:
sorted_car_docs = defaultdict(list)
for car_docs in all_car_docs:
sorted_car_docs[car_docs.make].append(car_docs)
return dict(sorted_car_docs)
def generate_cars_md(all_car_docs: list[CarDocs], template_fn: str, **kwargs) -> str:
with open(template_fn) as f:
template = jinja2.Template(f.read(), trim_blocks=True, lstrip_blocks=True)
footnotes = [fn.value.text for fn in get_all_footnotes()]
cars_md: str = template.render(all_car_docs=all_car_docs, PartType=PartType,
group_by_make=group_by_make, footnotes=footnotes,
Device=Device, Column=Column, ExtraCarsColumn=ExtraCarsColumn,
BaseCarHarness=BaseCarHarness, SupportType=SupportType,
**kwargs)
return cars_md
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Auto generates supportability info docs for all known cars",
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("--out", default=EXTRA_CARS_MD_OUT, help="Override default generated filename")
args = parser.parse_args()
with open(args.out, 'w') as f:
f.write(generate_cars_md(get_all_car_docs(), args.template))
print(f"Generated and written to {args.out}")

406
artifacts/package_runtime/iqdbc/car/docs_definitions.py Normal file
View File

@@ -0,0 +1,406 @@
import re
from collections import namedtuple
import copy
from dataclasses import dataclass, field
from enum import Enum
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.structs import CarParams
GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2
MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)"
class Column(Enum):
MAKE = "Make"
MODEL = "Model"
PACKAGE = "Supported Package"
LONGITUDINAL = "ACC"
FSR_LONGITUDINAL = "No ACC accel below"
FSR_STEERING = "No ALC below"
STEERING_TORQUE = "Steering Torque"
AUTO_RESUME = "Resume from stop"
HARDWARE = "Hardware Needed"
VIDEO = "Video"
SETUP_VIDEO = "Setup Video"
class ExtraCarsColumn(Enum):
MAKE = "Make"
MODEL = "Model"
PACKAGE = "Package"
SUPPORT = "Support Level"
class SupportType(Enum):
UPSTREAM = "Upstream" # Actively maintained by comma, plug-and-play in release versions of openpilot
REVIEW = "Under review" # Dashcam, but planned for official support after safety validation
DASHCAM = "Dashcam mode" # Dashcam, but may be drivable in a community fork
COMMUNITY = "Community" # Not upstream, but available in a custom community fork, not validated by comma
CUSTOM = "Custom" # Upstream, but don't have a harness available or need an unusual custom install
INCOMPATIBLE = "Not compatible" # Known fundamental incompatibility such as Flexray or hydraulic power steering
class Star(Enum):
FULL = "full"
HALF = "half"
EMPTY = "empty"
# A part + its comprised parts
@dataclass
class BasePart:
name: str
parts: list[Enum] = field(default_factory=list)
def all_parts(self):
# Recursively get all parts
_parts = 'parts'
parts = []
parts.extend(getattr(self, _parts))
for part in getattr(self, _parts):
parts.extend(part.value.all_parts())
return parts
class EnumBase(Enum):
@property
def part_type(self):
return PartType(self.__class__)
class Mount(EnumBase):
mount = BasePart("mount")
class Cable(EnumBase):
long_obdc_cable = BasePart("long OBD-C cable (9.5 ft)")
usb_a_2_a_cable = BasePart("USB A-A cable")
usbc_otg_cable = BasePart("USB C OTG cable")
obd_c_cable_2ft = BasePart("OBD-C cable (2 ft)")
class Accessory(EnumBase):
harness_box = BasePart("harness box")
comma_power = BasePart("comma power v3")
class Tool(EnumBase):
socket_8mm_deep = BasePart("Socket Wrench 8mm or 5/16\" (deep)")
pry_tool = BasePart("Pry Tool")
@dataclass
class BaseCarHarness(BasePart):
parts: list[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power])
has_connector: bool = True # without are hidden on the harness connector page
class CarHarness(EnumBase):
nidec = BaseCarHarness("Honda Nidec connector")
bosch_a = BaseCarHarness("Honda Bosch A connector")
bosch_b = BaseCarHarness("Honda Bosch B connector")
bosch_c = BaseCarHarness("Honda Bosch C connector")
toyota_a = BaseCarHarness("Toyota A connector")
toyota_b = BaseCarHarness("Toyota B connector")
subaru_a = BaseCarHarness("Subaru A connector", parts=[Accessory.harness_box, Accessory.comma_power, Tool.socket_8mm_deep, Tool.pry_tool])
subaru_b = BaseCarHarness("Subaru B connector", parts=[Accessory.harness_box, Accessory.comma_power, Tool.socket_8mm_deep, Tool.pry_tool])
subaru_c = BaseCarHarness("Subaru C connector", parts=[Accessory.harness_box, Accessory.comma_power, Tool.socket_8mm_deep, Tool.pry_tool])
subaru_d = BaseCarHarness("Subaru D connector", parts=[Accessory.harness_box, Accessory.comma_power, Tool.socket_8mm_deep, Tool.pry_tool])
fca = BaseCarHarness("FCA connector")
ram = BaseCarHarness("Ram connector")
vw_a = BaseCarHarness("VW A connector")
vw_j533 = BaseCarHarness("VW J533 connector", parts=[Accessory.harness_box, Cable.long_obdc_cable])
hyundai_a = BaseCarHarness("Hyundai A connector")
hyundai_b = BaseCarHarness("Hyundai B connector")
hyundai_c = BaseCarHarness("Hyundai C connector")
hyundai_d = BaseCarHarness("Hyundai D connector")
hyundai_e = BaseCarHarness("Hyundai E connector")
hyundai_f = BaseCarHarness("Hyundai F connector")
hyundai_g = BaseCarHarness("Hyundai G connector")
hyundai_h = BaseCarHarness("Hyundai H connector")
hyundai_i = BaseCarHarness("Hyundai I connector")
hyundai_j = BaseCarHarness("Hyundai J connector")
hyundai_k = BaseCarHarness("Hyundai K connector")
hyundai_l = BaseCarHarness("Hyundai L connector")
hyundai_m = BaseCarHarness("Hyundai M connector")
hyundai_n = BaseCarHarness("Hyundai N connector")
hyundai_o = BaseCarHarness("Hyundai O connector")
hyundai_p = BaseCarHarness("Hyundai P connector")
hyundai_q = BaseCarHarness("Hyundai Q connector")
hyundai_r = BaseCarHarness("Hyundai R connector")
custom = BaseCarHarness("Developer connector")
obd_ii = BaseCarHarness("OBD-II connector", parts=[Cable.long_obdc_cable], has_connector=False)
gm = BaseCarHarness("GM connector", parts=[Accessory.harness_box])
gmsdgm = BaseCarHarness("GM SDGM connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
nissan_a = BaseCarHarness("Nissan A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
mazda = BaseCarHarness("Mazda connector")
ford_q3 = BaseCarHarness("Ford Q3 connector")
ford_q4 = BaseCarHarness("Ford Q4 connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
rivian = BaseCarHarness("Rivian A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
tesla_a = BaseCarHarness("Tesla A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable])
tesla_b = BaseCarHarness("Tesla B connector", parts=[Accessory.harness_box, Cable.long_obdc_cable])
psa_a = BaseCarHarness("PSA A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable])
# custom harness
honda_clarity = BaseCarHarness("Honda Nidec connector + Honda Clarity Proxy Board")
class Device(EnumBase):
four = BasePart("comma four", parts=[Mount.mount, Cable.obd_c_cable_2ft])
class PartType(Enum):
accessory = Accessory
cable = Cable
connector = CarHarness
device = Device
mount = Mount
tool = Tool
DEFAULT_CAR_PARTS: list[EnumBase] = [Device.four]
@dataclass
class CarParts:
parts: list[EnumBase] = field(default_factory=list)
custom_parts_url: str | None = None
def __call__(self):
return copy.deepcopy(self)
@classmethod
def common(cls, add: list[EnumBase] | None = None, remove: list[EnumBase] | None = None):
p = [part for part in (add or []) + DEFAULT_CAR_PARTS if part not in (remove or [])]
return cls(p)
def all_parts(self):
parts = []
for part in self.parts:
parts.extend(part.value.all_parts())
return self.parts + parts
CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "setup_note"], defaults=(False, False))
class CommonFootnote(Enum):
EXP_LONG_AVAIL = CarFootnote(
"openpilot Longitudinal Control (Alpha) is available behind a toggle; " +
"the toggle is only available in non-release branches such as `devel` or `nightly-dev`.",
Column.LONGITUDINAL, docs_only=True)
def get_footnotes(footnotes: list[Enum], column: Column) -> list[Enum]:
# Returns applicable footnotes given current column
return [fn for fn in footnotes if fn.value.column == column]
# TODO: store years as a list
def get_year_list(years):
years_list = []
if len(years) == 0:
return years_list
for year in years.split(','):
year = year.strip()
if len(year) == 4:
years_list.append(str(year))
elif "-" in year and len(year) == 7:
start, end = year.split("-")
years_list.extend(map(str, range(int(start), int(f"20{end}") + 1)))
else:
raise Exception(f"Malformed year string: {years}")
return years_list
def split_name(name: str) -> tuple[str, str, str]:
make, model = name.split(" ", 1)
years = ""
match = re.search(MODEL_YEARS_RE, model)
if match is not None:
years = model[match.start():]
model = model[:match.start() - 1]
return make, model, years
@dataclass
class CarDocs:
# make + model + model years
name: str
# the simplest description of the requirements for the US market
package: str
video: str | None = None
setup_video: str | None = None
footnotes: list[Enum] = field(default_factory=list)
min_steer_speed: float | None = None
min_enable_speed: float | None = None
auto_resume: bool | None = None
# all the parts needed for the supported car
car_parts: CarParts = field(default_factory=CarParts)
merged: bool = True
support_type: SupportType = SupportType.UPSTREAM
support_link: str | None = "#upstream"
def __post_init__(self):
self.make, self.model, self.years = split_name(self.name)
self.year_list = get_year_list(self.years)
def init(self, CP: CarParams, all_footnotes=None):
self.brand = CP.brand
self.car_fingerprint = CP.carFingerprint
self.longitudinal_control = CP.openpilotLongitudinalControl and not CP.alphaLongitudinalAvailable
if self.merged and CP.dashcamOnly:
if self.support_type not in (SupportType.CUSTOM, SupportType.REVIEW):
self.support_type = SupportType.DASHCAM
self.support_link = "#dashcam"
# longitudinal column
op_long = "Stock"
if CP.alphaLongitudinalAvailable:
op_long = "openpilot available"
self.footnotes.append(CommonFootnote.EXP_LONG_AVAIL)
elif CP.openpilotLongitudinalControl:
op_long = "openpilot"
# min steer & enable speed columns
# TODO: set all the min steer speeds in carParams and remove this
if self.min_steer_speed is not None:
assert CP.minSteerSpeed < 0.5, f"{CP.carFingerprint}: Minimum steer speed set in both CarDocs and CarParams"
else:
self.min_steer_speed = CP.minSteerSpeed
# TODO: set all the min enable speeds in carParams correctly and remove this
if self.min_enable_speed is None:
self.min_enable_speed = CP.minEnableSpeed
if self.auto_resume is None:
self.auto_resume = CP.autoResumeSng and self.min_enable_speed <= 0
# hardware column
hardware_col = "None"
if self.car_parts.parts:
if self.car_parts.custom_parts_url is not None:
buy_link = f'<a href="{self.car_parts.custom_parts_url}">Buy Here</a>'
else:
buy_link = f'<a href="https://comma.ai/shop/comma-3x?harness={self.name}">Buy Here</a>'
tools_docs = [part for part in self.car_parts.all_parts() if isinstance(part, Tool)]
parts_docs = [part for part in self.car_parts.all_parts() if not isinstance(part, Tool)]
def display_func(parts):
return '<br>'.join([f"- {parts.count(part)} {part.value.name}" for part in sorted(set(parts), key=lambda part: str(part.value.name))])
hardware_col = f'<details><summary>Parts</summary><sub>{display_func(parts_docs)}<br>{buy_link}</sub></details>'
if len(tools_docs):
hardware_col += f'<details><summary>Tools</summary><sub>{display_func(tools_docs)}</sub></details>'
self.row: dict[Enum, str | Star] = {
Column.MAKE: self.make,
Column.MODEL: self.model,
Column.PACKAGE: self.package,
Column.LONGITUDINAL: op_long,
Column.FSR_LONGITUDINAL: f"{max(self.min_enable_speed * CV.MS_TO_MPH, 0):.0f} mph",
Column.FSR_STEERING: f"{max(self.min_steer_speed * CV.MS_TO_MPH, 0):.0f} mph",
Column.STEERING_TORQUE: Star.EMPTY,
Column.AUTO_RESUME: Star.FULL if self.auto_resume else Star.EMPTY,
Column.HARDWARE: hardware_col,
Column.VIDEO: self.video or "", # replaced with an image and link from template in get_column
Column.SETUP_VIDEO: self.setup_video or "", # replaced with an image and link from template in get_column
}
if self.support_link is not None:
support_info = f"[{self.support_type.value}]({self.support_link})"
else:
support_info = self.support_type.value
self.extra_cars_row: dict[Enum, str] = {
ExtraCarsColumn.MAKE: self.make,
ExtraCarsColumn.MODEL: self.model,
ExtraCarsColumn.PACKAGE: self.package,
ExtraCarsColumn.SUPPORT: support_info,
}
# Set steering torque star from max lateral acceleration
assert CP.maxLateralAccel > 0.1
if CP.maxLateralAccel >= GOOD_TORQUE_THRESHOLD:
self.row[Column.STEERING_TORQUE] = Star.FULL
self.all_footnotes = all_footnotes
self.detail_sentence = self.get_detail_sentence(CP)
return self
def init_make(self, CP: CarParams):
"""CarDocs subclasses can add make-specific logic for harness selection, footnotes, etc."""
def get_detail_sentence(self, CP):
if not CP.notCar:
sentence_builder = "openpilot upgrades your <strong>{car_model}</strong> with automated lane centering{alc} and adaptive cruise control{acc}."
if self.min_steer_speed > self.min_enable_speed:
alc = f" <strong>above {self.min_steer_speed * CV.MS_TO_MPH:.0f} mph</strong>," if self.min_steer_speed > 0 else " <strong>at all speeds</strong>,"
else:
alc = ""
# Exception for cars which do not auto-resume yet
acc = ""
if self.min_enable_speed > 0:
acc = f" <strong>while driving above {self.min_enable_speed * CV.MS_TO_MPH:.0f} mph</strong>"
elif self.auto_resume:
acc = " <strong>that automatically resumes from a stop</strong>"
if self.row[Column.STEERING_TORQUE] != Star.FULL:
sentence_builder += " This car may not be able to take tight turns on its own."
# experimental mode
exp_link = "<a href='https://blog.comma.ai/090release/#experimental-mode' target='_blank' class='highlight'>Experimental mode</a>"
if CP.openpilotLongitudinalControl and not CP.alphaLongitudinalAvailable:
sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}."
return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc)
else:
if CP.carFingerprint == "COMMA_BODY":
return "The body is a robotics dev kit that can run openpilot. <a href='https://www.commabody.com' target='_blank' class='highlight'>Learn more.</a>"
else:
raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}")
def get_column(self, column: Column, star_icon: str, video_icon: str, footnote_tag: str) -> str:
item: str | Star = self.row[column]
if isinstance(item, Star):
item = star_icon.format(item.value)
elif column == Column.MODEL and len(self.years):
item += f" {self.years}"
elif column in (Column.VIDEO, Column.SETUP_VIDEO) and len(item) > 0:
item = video_icon.format(item)
footnotes = get_footnotes(self.footnotes, column)
if len(footnotes):
sups = sorted([self.all_footnotes[fn] for fn in footnotes])
item += footnote_tag.format(f'{",".join(map(str, sups))}')
return item
def get_extra_cars_column(self, column: ExtraCarsColumn) -> str:
item: str = self.extra_cars_row[column]
if column == ExtraCarsColumn.MODEL and len(self.years):
item += f" {self.years}"
return item
@dataclass
class ExtraCarDocs(CarDocs):
package: str = "All"
merged: bool = False
support_type: SupportType = SupportType.INCOMPATIBLE
support_link: str | None = "#incompatible"

56
artifacts/package_runtime/iqdbc/car/ecu_addrs.py Normal file
View File

@@ -0,0 +1,56 @@
import time
from iqdbc.car import make_tester_present_msg, uds
from iqdbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
from iqdbc.car.carlog import carlog
from iqdbc.car.fw_query_definitions import EcuAddrBusType
def _is_tester_present_response(msg: CanData, subaddr: int | None = None) -> bool:
# ISO-TP messages may use CAN frame optimization (not always 8 bytes)
# tester present response is always a single frame
dat_offset = 1 if subaddr is not None else 0
min_length = 4 if subaddr is not None else 3 # bytes: frame len, (pos/neg) sid, (optional negative sid)/0x00 sub-function
if min_length <= len(msg.dat) <= 8 and 1 <= msg.dat[dat_offset] <= 7:
# success response
if msg.dat[dat_offset + 1] == (uds.SERVICE_TYPE.TESTER_PRESENT + 0x40):
return True
# error response
if msg.dat[dat_offset + 1] == 0x7F and msg.dat[dat_offset + 2] == uds.SERVICE_TYPE.TESTER_PRESENT:
return True
return False
def get_all_ecu_addrs(can_recv: CanRecvCallable, can_send: CanSendCallable, bus: int, timeout: float = 1) -> set[EcuAddrBusType]:
addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)]
queries: set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list}
responses = queries
return get_ecu_addrs(can_recv, can_send, queries, responses, timeout=timeout)
def get_ecu_addrs(can_recv: CanRecvCallable, can_send: CanSendCallable, queries: set[EcuAddrBusType],
responses: set[EcuAddrBusType], timeout: float = 1) -> set[EcuAddrBusType]:
ecu_responses: set[EcuAddrBusType] = set() # set((addr, subaddr, bus),)
try:
msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries]
can_recv()
can_send(msgs)
start_time = time.monotonic()
while time.monotonic() - start_time < timeout:
can_packets = can_recv(wait_for_one=True)
for packet in can_packets:
for msg in packet:
if not len(msg.dat):
carlog.warning("ECU addr scan: skipping empty remote frame")
continue
subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0]
if (msg.address, subaddr, msg.src) in responses and _is_tester_present_response(msg, subaddr):
carlog.debug(f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}")
if (msg.address, subaddr, msg.src) in ecu_responses:
carlog.debug(f"Duplicate ECU address: {hex(msg.address)}")
ecu_responses.add((msg.address, subaddr, msg.src))
except Exception:
carlog.exception("ECU addr scan exception")
return ecu_responses

88
artifacts/package_runtime/iqdbc/car/extra_cars.py Normal file
View File

@@ -0,0 +1,88 @@
from dataclasses import dataclass
from iqdbc.car import structs, Platforms, ExtraPlatformConfig
from iqdbc.car.docs_definitions import ExtraCarDocs, SupportType
@dataclass
class CommunityCarDocs(ExtraCarDocs):
def init_make(self, CP: structs.CarParams):
self.support_type = SupportType.COMMUNITY
self.support_link = "#community"
@dataclass
class ToyotaSecurityCarDocs(ExtraCarDocs):
def init_make(self, CP: structs.CarParams):
self.support_type = SupportType.INCOMPATIBLE
self.support_link = "#can-bus-security"
@dataclass
class GMSecurityCarDocs(ExtraCarDocs):
def init_make(self, CP: structs.CarParams):
self.support_type = SupportType.INCOMPATIBLE
self.support_link = "#can-bus-security"
@dataclass
class FlexRayCarDocs(ExtraCarDocs):
def init_make(self, CP: structs.CarParams):
self.support_type = SupportType.INCOMPATIBLE
self.support_link = "#flexray"
class CAR(Platforms):
config: ExtraPlatformConfig
EXTRA_HONDA = ExtraPlatformConfig(
[
CommunityCarDocs("Acura ADX 2025-26"),
CommunityCarDocs("Acura Integra 2023-25"),
CommunityCarDocs("Acura MDX 2015-16", "Advance Package"),
CommunityCarDocs("Acura MDX 2017-20"),
CommunityCarDocs("Acura MDX Hybrid 2017-20"),
CommunityCarDocs("Acura MDX 2022-24"),
CommunityCarDocs("Acura RDX 2022-25"),
CommunityCarDocs("Acura RLX 2017", "Advance Package or Technology Package"),
CommunityCarDocs("Acura TLX 2015-17", "Advance Package"),
CommunityCarDocs("Acura TLX 2018-20"),
CommunityCarDocs("Acura TLX 2022-23"),
GMSecurityCarDocs("Acura ZDX 2024"),
CommunityCarDocs("Honda Accord 2016-17", "Honda Sensing"),
CommunityCarDocs("Honda Accord Hybrid 2017"),
CommunityCarDocs("Honda Clarity 2018-21"),
GMSecurityCarDocs("Honda Prologue 2024-25"),
],
)
EXTRA_HYUNDAI = ExtraPlatformConfig(
[
CommunityCarDocs("Hyundai Palisade 2023-24", "Highway Driving Assist II"),
CommunityCarDocs("Kia Telluride 2023-24", "Highway Driving Assist II"),
],
)
EXTRA_TOYOTA = ExtraPlatformConfig(
[
ToyotaSecurityCarDocs("Subaru Solterra 2023-25"),
ToyotaSecurityCarDocs("Lexus NS 2022-25"),
ToyotaSecurityCarDocs("Toyota bZ4x 2023-25"),
ToyotaSecurityCarDocs("Toyota Camry 2025"),
ToyotaSecurityCarDocs("Toyota Corolla Cross 2022-25"),
ToyotaSecurityCarDocs("Toyota Highlander 2025"),
ToyotaSecurityCarDocs("Toyota RAV4 Prime 2024-25"),
ToyotaSecurityCarDocs("Toyota Sequoia 2023-25"),
ToyotaSecurityCarDocs("Toyota Sienna 2024-25"),
ToyotaSecurityCarDocs("Toyota Tundra 2022-25"),
ToyotaSecurityCarDocs("Toyota Venza 2021-25"),
],
)
EXTRA_VOLKSWAGEN = ExtraPlatformConfig(
[
FlexRayCarDocs("Audi A4 2016-24"),
FlexRayCarDocs("Audi A5 2016-24"),
FlexRayCarDocs("Audi Q5 2017-24"),
],
)

341
artifacts/package_runtime/iqdbc/car/fingerprints.py Normal file
View File

@@ -0,0 +1,341 @@
from iqdbc.car.interfaces import get_interface_attr
from iqdbc.car.body.values import CAR as BODY
from iqdbc.car.chrysler.values import CAR as CHRYSLER
from iqdbc.car.ford.values import CAR as FORD
from iqdbc.car.gm.values import CAR as GM
from iqdbc.car.honda.values import CAR as HONDA
from iqdbc.car.hyundai.values import CAR as HYUNDAI
from iqdbc.car.mazda.values import CAR as MAZDA
from iqdbc.car.mock.values import CAR as MOCK
from iqdbc.car.nissan.values import CAR as NISSAN
from iqdbc.car.subaru.values import CAR as SUBARU
from iqdbc.car.toyota.values import CAR as TOYOTA
from iqdbc.car.volkswagen.values import CAR as VW
FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True)
_FINGERPRINTS = get_interface_attr('FINGERPRINTS', combine_brands=True, ignore_none=True)
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
def is_valid_for_fingerprint(msg, car_fingerprint: dict[int, int]):
adr = msg.address
# ignore addresses that are more than 11 bits
return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or adr >= 0x800
def eliminate_incompatible_cars(msg, candidate_cars):
"""Removes cars that could not have sent msg.
Inputs:
msg: A cereal/log CanData message from the car.
candidate_cars: A list of cars to consider.
Returns:
A list containing the subset of candidate_cars that could have sent msg.
"""
compatible_cars = []
for car_name in candidate_cars:
car_fingerprints = _FINGERPRINTS[car_name]
for fingerprint in car_fingerprints:
# add alien debug address
if is_valid_for_fingerprint(msg, fingerprint | _DEBUG_ADDRESS):
compatible_cars.append(car_name)
break
return compatible_cars
def all_legacy_fingerprint_cars():
"""Returns a list of all known car strings, FPv1 only."""
return list(_FINGERPRINTS.keys())
# A dict that maps old platform strings to their latest representations
MIGRATION = {
"ACURA ILX 2016 ACURAWATCH PLUS": HONDA.ACURA_ILX,
"ACURA RDX 2018 ACURAWATCH PLUS": HONDA.ACURA_RDX,
"ACURA RDX 2020 TECH": HONDA.ACURA_RDX_3G,
"AUDI A3": VW.AUDI_A3_MK3,
"HONDA ACCORD 2018 HYBRID TOURING": HONDA.HONDA_ACCORD,
"HONDA ACCORD 1.5T 2018": HONDA.HONDA_ACCORD,
"HONDA ACCORD 2018 LX 1.5T": HONDA.HONDA_ACCORD,
"HONDA ACCORD 2018 SPORT 2T": HONDA.HONDA_ACCORD,
"HONDA ACCORD 2T 2018": HONDA.HONDA_ACCORD,
"HONDA ACCORD HYBRID 2018": HONDA.HONDA_ACCORD,
"HONDA CIVIC 2016 TOURING": HONDA.HONDA_CIVIC,
"HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019": HONDA.HONDA_CIVIC_BOSCH,
"HONDA CIVIC SEDAN 1.6 DIESEL": HONDA.HONDA_CIVIC_BOSCH_DIESEL,
"HONDA CR-V 2016 EXECUTIVE": HONDA.HONDA_CRV_EU,
"HONDA CR-V 2016 TOURING": HONDA.HONDA_CRV,
"HONDA CR-V 2017 EX": HONDA.HONDA_CRV_5G,
"HONDA CR-V 2019 HYBRID": HONDA.HONDA_CRV_HYBRID,
"HONDA FIT 2018 EX": HONDA.HONDA_FIT,
"HONDA HRV 2019 TOURING": HONDA.HONDA_HRV,
"HONDA INSIGHT 2019 TOURING": HONDA.HONDA_INSIGHT,
"HONDA ODYSSEY 2018 EX-L": HONDA.HONDA_ODYSSEY,
"HONDA PILOT 2017 TOURING": HONDA.HONDA_PILOT,
"HONDA PILOT 2019 ELITE": HONDA.HONDA_PILOT,
"HONDA PILOT 2019": HONDA.HONDA_PILOT,
"HONDA PASSPORT 2021": HONDA.HONDA_PILOT,
"HONDA RIDGELINE 2017 BLACK EDITION": HONDA.HONDA_RIDGELINE,
"HYUNDAI ELANTRA LIMITED ULTIMATE 2017": HYUNDAI.HYUNDAI_ELANTRA,
"HYUNDAI SANTA FE LIMITED 2019": HYUNDAI.HYUNDAI_SANTA_FE,
"HYUNDAI TUCSON DIESEL 2019": HYUNDAI.HYUNDAI_TUCSON,
"KIA OPTIMA 2016": HYUNDAI.KIA_OPTIMA_G4,
"KIA OPTIMA 2019": HYUNDAI.KIA_OPTIMA_G4_FL,
"KIA OPTIMA SX 2019 & 2016": HYUNDAI.KIA_OPTIMA_G4_FL,
"LEXUS CT 200H 2018": TOYOTA.LEXUS_CTH,
"LEXUS ES 300H 2018": TOYOTA.LEXUS_ES,
"LEXUS ES 300H 2019": TOYOTA.LEXUS_ES_TSS2,
"LEXUS IS300 2018": TOYOTA.LEXUS_IS,
"LEXUS NX300 2018": TOYOTA.LEXUS_NX,
"LEXUS NX300H 2018": TOYOTA.LEXUS_NX,
"LEXUS RX 350 2016": TOYOTA.LEXUS_RX,
"LEXUS RX350 2020": TOYOTA.LEXUS_RX_TSS2,
"LEXUS RX450 HYBRID 2020": TOYOTA.LEXUS_RX_TSS2,
"TOYOTA SIENNA XLE 2018": TOYOTA.TOYOTA_SIENNA,
"TOYOTA C-HR HYBRID 2018": TOYOTA.TOYOTA_CHR,
"TOYOTA COROLLA HYBRID TSS2 2019": TOYOTA.TOYOTA_COROLLA_TSS2,
"TOYOTA RAV4 HYBRID 2019": TOYOTA.TOYOTA_RAV4_TSS2,
"LEXUS ES HYBRID 2019": TOYOTA.LEXUS_ES_TSS2,
"LEXUS NX HYBRID 2018": TOYOTA.LEXUS_NX,
"LEXUS NX HYBRID 2020": TOYOTA.LEXUS_NX_TSS2,
"LEXUS RX HYBRID 2020": TOYOTA.LEXUS_RX_TSS2,
"TOYOTA ALPHARD HYBRID 2021": TOYOTA.TOYOTA_ALPHARD_TSS2,
"TOYOTA AVALON HYBRID 2019": TOYOTA.TOYOTA_AVALON_2019,
"TOYOTA AVALON HYBRID 2022": TOYOTA.TOYOTA_AVALON_TSS2,
"TOYOTA CAMRY HYBRID 2018": TOYOTA.TOYOTA_CAMRY,
"TOYOTA CAMRY HYBRID 2021": TOYOTA.TOYOTA_CAMRY_TSS2,
"TOYOTA C-HR HYBRID 2022": TOYOTA.TOYOTA_CHR_TSS2,
"TOYOTA HIGHLANDER HYBRID 2020": TOYOTA.TOYOTA_HIGHLANDER_TSS2,
"TOYOTA RAV4 HYBRID 2022": TOYOTA.TOYOTA_RAV4_TSS2_2022,
"TOYOTA RAV4 HYBRID 2023": TOYOTA.TOYOTA_RAV4_TSS2_2023,
"TOYOTA HIGHLANDER HYBRID 2018": TOYOTA.TOYOTA_HIGHLANDER,
"LEXUS ES HYBRID 2018": TOYOTA.LEXUS_ES,
"LEXUS RX HYBRID 2017": TOYOTA.LEXUS_RX,
"HYUNDAI TUCSON HYBRID 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN,
"KIA SPORTAGE HYBRID 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN,
"KIA SORENTO PLUG-IN HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN,
"CADILLAC ESCALADE ESV PLATINUM 2019": GM.CADILLAC_ESCALADE_ESV_2019,
# Removal of platform_str, see https://github.com/commaai/openpilot/pull/31868/
"COMMA BODY": BODY.COMMA_BODY,
"CHRYSLER PACIFICA HYBRID 2017": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID,
"CHRYSLER_PACIFICA_2017_HYBRID": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID,
"CHRYSLER PACIFICA HYBRID 2018": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID,
"CHRYSLER PACIFICA HYBRID 2019": CHRYSLER.CHRYSLER_PACIFICA_2019_HYBRID,
"CHRYSLER PACIFICA 2018": CHRYSLER.CHRYSLER_PACIFICA_2018,
"CHRYSLER PACIFICA 2020": CHRYSLER.CHRYSLER_PACIFICA_2020,
"DODGE DURANGO 2021": CHRYSLER.DODGE_DURANGO,
"JEEP GRAND CHEROKEE V6 2018": CHRYSLER.JEEP_GRAND_CHEROKEE,
"JEEP GRAND CHEROKEE 2019": CHRYSLER.JEEP_GRAND_CHEROKEE_2019,
"RAM 1500 5TH GEN": CHRYSLER.RAM_1500_5TH_GEN,
"RAM HD 5TH GEN": CHRYSLER.RAM_HD_5TH_GEN,
"FORD BRONCO SPORT 1ST GEN": FORD.FORD_BRONCO_SPORT_MK1,
"FORD ESCAPE 4TH GEN": FORD.FORD_ESCAPE_MK4,
"FORD EXPLORER 6TH GEN": FORD.FORD_EXPLORER_MK6,
"FORD F-150 14TH GEN": FORD.FORD_F_150_MK14,
"FORD F-150 LIGHTNING 1ST GEN": FORD.FORD_F_150_LIGHTNING_MK1,
"FORD FOCUS 4TH GEN": FORD.FORD_FOCUS_MK4,
"FORD MAVERICK 1ST GEN": FORD.FORD_MAVERICK_MK1,
"FORD MUSTANG MACH-E 1ST GEN": FORD.FORD_MUSTANG_MACH_E_MK1,
"HOLDEN ASTRA RS-V BK 2017": GM.HOLDEN_ASTRA,
"CHEVROLET VOLT PREMIER 2017": GM.CHEVROLET_VOLT,
"CADILLAC ATS Premium Performance 2018": GM.CADILLAC_ATS,
"CHEVROLET MALIBU PREMIER 2017": GM.CHEVROLET_MALIBU,
"GMC ACADIA DENALI 2018": GM.GMC_ACADIA,
"BUICK LACROSSE 2017": GM.BUICK_LACROSSE,
"BUICK REGAL ESSENCE 2018": GM.BUICK_REGAL,
"CADILLAC ESCALADE 2017": GM.CADILLAC_ESCALADE,
"CADILLAC ESCALADE ESV 2016": GM.CADILLAC_ESCALADE_ESV,
"CADILLAC ESCALADE ESV 2019": GM.CADILLAC_ESCALADE_ESV_2019,
"CHEVROLET BOLT EUV 2022": GM.CHEVROLET_BOLT_EUV,
"CHEVROLET SILVERADO 1500 2020": GM.CHEVROLET_SILVERADO,
"CHEVROLET EQUINOX 2019": GM.CHEVROLET_EQUINOX,
"CHEVROLET TRAILBLAZER 2021": GM.CHEVROLET_TRAILBLAZER,
"HONDA ACCORD 2018": HONDA.HONDA_ACCORD,
"HONDA CIVIC (BOSCH) 2019": HONDA.HONDA_CIVIC_BOSCH,
"HONDA CIVIC SEDAN 1.6 DIESEL 2019": HONDA.HONDA_CIVIC_BOSCH_DIESEL,
"HONDA CIVIC 2022": HONDA.HONDA_CIVIC_2022,
"HONDA CR-V 2017": HONDA.HONDA_CRV_5G,
"HONDA CR-V HYBRID 2019": HONDA.HONDA_CRV_HYBRID,
"HONDA HR-V 2023": HONDA.HONDA_HRV_3G,
"ACURA RDX 2020": HONDA.ACURA_RDX_3G,
"HONDA INSIGHT 2019": HONDA.HONDA_INSIGHT,
"HONDA E 2020": HONDA.HONDA_E,
"ACURA ILX 2016": HONDA.ACURA_ILX,
"HONDA CR-V 2016": HONDA.HONDA_CRV,
"HONDA CR-V EU 2016": HONDA.HONDA_CRV_EU,
"HONDA FIT 2018": HONDA.HONDA_FIT,
"HONDA FREED 2020": HONDA.HONDA_FREED,
"HONDA HRV 2019": HONDA.HONDA_HRV,
"HONDA ODYSSEY 2018": HONDA.HONDA_ODYSSEY,
"ACURA RDX 2018": HONDA.ACURA_RDX,
"HONDA PILOT 2017": HONDA.HONDA_PILOT,
"HONDA RIDGELINE 2017": HONDA.HONDA_RIDGELINE,
"HONDA CIVIC 2016": HONDA.HONDA_CIVIC,
"HYUNDAI AZERA 6TH GEN": HYUNDAI.HYUNDAI_AZERA_6TH_GEN,
"HYUNDAI AZERA HYBRID 6TH GEN": HYUNDAI.HYUNDAI_AZERA_HEV_6TH_GEN,
"HYUNDAI ELANTRA 2017": HYUNDAI.HYUNDAI_ELANTRA,
"HYUNDAI I30 N LINE 2019 & GT 2018 DCT": HYUNDAI.HYUNDAI_ELANTRA_GT_I30,
"HYUNDAI ELANTRA 2021": HYUNDAI.HYUNDAI_ELANTRA_2021,
"HYUNDAI ELANTRA HYBRID 2021": HYUNDAI.HYUNDAI_ELANTRA_HEV_2021,
"HYUNDAI GENESIS 2015-2016": HYUNDAI.HYUNDAI_GENESIS,
"HYUNDAI IONIQ HYBRID 2017-2019": HYUNDAI.HYUNDAI_IONIQ,
"HYUNDAI IONIQ HYBRID 2020-2022": HYUNDAI.HYUNDAI_IONIQ_HEV_2022,
"HYUNDAI IONIQ ELECTRIC LIMITED 2019": HYUNDAI.HYUNDAI_IONIQ_EV_LTD,
"HYUNDAI IONIQ ELECTRIC 2020": HYUNDAI.HYUNDAI_IONIQ_EV_2020,
"HYUNDAI IONIQ PLUG-IN HYBRID 2019": HYUNDAI.HYUNDAI_IONIQ_PHEV_2019,
"HYUNDAI IONIQ PHEV 2020": HYUNDAI.HYUNDAI_IONIQ_PHEV,
"HYUNDAI KONA 2020": HYUNDAI.HYUNDAI_KONA,
"HYUNDAI KONA ELECTRIC 2019": HYUNDAI.HYUNDAI_KONA_EV,
"HYUNDAI KONA ELECTRIC 2022": HYUNDAI.HYUNDAI_KONA_EV_2022,
"HYUNDAI KONA ELECTRIC 2ND GEN": HYUNDAI.HYUNDAI_KONA_EV_2ND_GEN,
"HYUNDAI KONA HYBRID 2020": HYUNDAI.HYUNDAI_KONA_HEV,
"HYUNDAI SANTA FE 2019": HYUNDAI.HYUNDAI_SANTA_FE,
"HYUNDAI SANTA FE 2022": HYUNDAI.HYUNDAI_SANTA_FE_2022,
"HYUNDAI SANTA FE HYBRID 2022": HYUNDAI.HYUNDAI_SANTA_FE_HEV_2022,
"HYUNDAI SANTA FE PlUG-IN HYBRID 2022": HYUNDAI.HYUNDAI_SANTA_FE_PHEV_2022,
"HYUNDAI SONATA 2020": HYUNDAI.HYUNDAI_SONATA,
"HYUNDAI SONATA 2019": HYUNDAI.HYUNDAI_SONATA_LF,
"HYUNDAI STARIA 4TH GEN": HYUNDAI.HYUNDAI_STARIA_4TH_GEN,
"HYUNDAI TUCSON 2019": HYUNDAI.HYUNDAI_TUCSON,
"HYUNDAI PALISADE 2020": HYUNDAI.HYUNDAI_PALISADE,
"HYUNDAI VELOSTER 2019": HYUNDAI.HYUNDAI_VELOSTER,
"HYUNDAI SONATA HYBRID 2021": HYUNDAI.HYUNDAI_SONATA_HYBRID,
"HYUNDAI IONIQ 5 2022": HYUNDAI.HYUNDAI_IONIQ_5,
"HYUNDAI IONIQ 6 2023": HYUNDAI.HYUNDAI_IONIQ_6,
"HYUNDAI TUCSON 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN,
"HYUNDAI SANTA CRUZ 1ST GEN": HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN,
"HYUNDAI CUSTIN 1ST GEN": HYUNDAI.HYUNDAI_CUSTIN_1ST_GEN,
"KIA FORTE E 2018 & GT 2021": HYUNDAI.KIA_FORTE,
"KIA K5 2021": HYUNDAI.KIA_K5_2021,
"KIA K5 HYBRID 2020": HYUNDAI.KIA_K5_HEV_2020,
"KIA K8 HYBRID 1ST GEN": HYUNDAI.KIA_K8_HEV_1ST_GEN,
"KIA NIRO EV 2020": HYUNDAI.KIA_NIRO_EV,
"KIA NIRO EV 2ND GEN": HYUNDAI.KIA_NIRO_EV_2ND_GEN,
"KIA NIRO HYBRID 2019": HYUNDAI.KIA_NIRO_PHEV,
"KIA NIRO PLUG-IN HYBRID 2022": HYUNDAI.KIA_NIRO_PHEV_2022,
"KIA NIRO HYBRID 2021": HYUNDAI.KIA_NIRO_HEV_2021,
"KIA NIRO HYBRID 2ND GEN": HYUNDAI.KIA_NIRO_HEV_2ND_GEN,
"KIA OPTIMA 4TH GEN": HYUNDAI.KIA_OPTIMA_G4,
"KIA OPTIMA 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_G4_FL,
"KIA OPTIMA HYBRID 2017 & SPORTS 2019": HYUNDAI.KIA_OPTIMA_H,
"KIA OPTIMA HYBRID 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_H_G4_FL,
"KIA SELTOS 2021": HYUNDAI.KIA_SELTOS,
"KIA SPORTAGE 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN,
"KIA SORENTO GT LINE 2018": HYUNDAI.KIA_SORENTO,
"KIA SORENTO 4TH GEN": HYUNDAI.KIA_SORENTO_4TH_GEN,
"KIA SORENTO HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN,
"KIA STINGER GT2 2018": HYUNDAI.KIA_STINGER,
"KIA STINGER 2022": HYUNDAI.KIA_STINGER_2022,
"KIA CEED INTRO ED 2019": HYUNDAI.KIA_CEED,
"KIA EV6 2022": HYUNDAI.KIA_EV6,
"KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN,
"GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN,
"GENESIS G70 2018": HYUNDAI.GENESIS_G70,
"GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020,
"GENESIS GV70 1ST GEN": HYUNDAI.GENESIS_GV70_1ST_GEN,
"GENESIS G80 2017": HYUNDAI.GENESIS_G80,
"GENESIS G90 2017": HYUNDAI.GENESIS_G90,
"GENESIS GV80 2023": HYUNDAI.GENESIS_GV80,
"MAZDA CX-5": MAZDA.MAZDA_CX5,
"MAZDA CX-9": MAZDA.MAZDA_CX9,
"MAZDA 3": MAZDA.MAZDA_3,
"MAZDA 6": MAZDA.MAZDA_6,
"MAZDA CX-9 2021": MAZDA.MAZDA_CX9_2021,
"MAZDA CX-5 2022": MAZDA.MAZDA_CX5_2022,
"NISSAN X-TRAIL 2017": NISSAN.NISSAN_XTRAIL,
"NISSAN LEAF 2018": NISSAN.NISSAN_LEAF,
"NISSAN LEAF 2018 Instrument Cluster": NISSAN.NISSAN_LEAF_IC,
"NISSAN ROGUE 2019": NISSAN.NISSAN_ROGUE,
"NISSAN ALTIMA 2020": NISSAN.NISSAN_ALTIMA,
"SUBARU ASCENT LIMITED 2019": SUBARU.SUBARU_ASCENT,
"SUBARU OUTBACK 6TH GEN": SUBARU.SUBARU_OUTBACK,
"SUBARU LEGACY 7TH GEN": SUBARU.SUBARU_LEGACY,
"SUBARU IMPREZA LIMITED 2019": SUBARU.SUBARU_IMPREZA,
"SUBARU IMPREZA SPORT 2020": SUBARU.SUBARU_IMPREZA_2020,
"SUBARU CROSSTREK HYBRID 2020": SUBARU.SUBARU_CROSSTREK_HYBRID,
"SUBARU FORESTER 2019": SUBARU.SUBARU_FORESTER,
"SUBARU FORESTER HYBRID 2020": SUBARU.SUBARU_FORESTER_HYBRID,
"SUBARU FORESTER 2017 - 2018": SUBARU.SUBARU_FORESTER_PREGLOBAL,
"SUBARU LEGACY 2015 - 2018": SUBARU.SUBARU_LEGACY_PREGLOBAL,
"SUBARU OUTBACK 2015 - 2017": SUBARU.SUBARU_OUTBACK_PREGLOBAL,
"SUBARU OUTBACK 2018 - 2019": SUBARU.SUBARU_OUTBACK_PREGLOBAL_2018,
"SUBARU FORESTER 2022": SUBARU.SUBARU_FORESTER_2022,
"SUBARU OUTBACK 7TH GEN": SUBARU.SUBARU_OUTBACK_2023,
"SUBARU ASCENT 2023": SUBARU.SUBARU_ASCENT_2023,
"TOYOTA ALPHARD 2020": TOYOTA.TOYOTA_ALPHARD_TSS2,
"TOYOTA AVALON 2016": TOYOTA.TOYOTA_AVALON,
"TOYOTA AVALON 2019": TOYOTA.TOYOTA_AVALON_2019,
"TOYOTA AVALON 2022": TOYOTA.TOYOTA_AVALON_TSS2,
"TOYOTA CAMRY 2018": TOYOTA.TOYOTA_CAMRY,
"TOYOTA CAMRY 2021": TOYOTA.TOYOTA_CAMRY_TSS2,
"TOYOTA C-HR 2018": TOYOTA.TOYOTA_CHR,
"TOYOTA C-HR 2021": TOYOTA.TOYOTA_CHR_TSS2,
"TOYOTA COROLLA 2017": TOYOTA.TOYOTA_COROLLA,
"TOYOTA COROLLA TSS2 2019": TOYOTA.TOYOTA_COROLLA_TSS2,
"TOYOTA HIGHLANDER 2017": TOYOTA.TOYOTA_HIGHLANDER,
"TOYOTA HIGHLANDER 2020": TOYOTA.TOYOTA_HIGHLANDER_TSS2,
"TOYOTA PRIUS 2017": TOYOTA.TOYOTA_PRIUS,
"TOYOTA PRIUS v 2017": TOYOTA.TOYOTA_PRIUS_V,
"TOYOTA PRIUS TSS2 2021": TOYOTA.TOYOTA_PRIUS_TSS2,
"TOYOTA RAV4 2017": TOYOTA.TOYOTA_RAV4,
"TOYOTA RAV4 HYBRID 2017": TOYOTA.TOYOTA_RAV4H,
"TOYOTA RAV4 2019": TOYOTA.TOYOTA_RAV4_TSS2,
"TOYOTA RAV4 2022": TOYOTA.TOYOTA_RAV4_TSS2_2022,
"TOYOTA RAV4 2023": TOYOTA.TOYOTA_RAV4_TSS2_2023,
"TOYOTA MIRAI 2021": TOYOTA.TOYOTA_MIRAI,
"TOYOTA SIENNA 2018": TOYOTA.TOYOTA_SIENNA,
"LEXUS CT HYBRID 2018": TOYOTA.LEXUS_CTH,
"LEXUS ES 2018": TOYOTA.LEXUS_ES,
"LEXUS ES 2019": TOYOTA.LEXUS_ES_TSS2,
"LEXUS IS 2018": TOYOTA.LEXUS_IS,
"LEXUS IS 2023": TOYOTA.LEXUS_IS_TSS2,
"LEXUS NX 2018": TOYOTA.LEXUS_NX,
"LEXUS NX 2020": TOYOTA.LEXUS_NX_TSS2,
"LEXUS LC 2024": TOYOTA.LEXUS_LC_TSS2,
"LEXUS RC 2020": TOYOTA.LEXUS_RC,
"LEXUS RX 2016": TOYOTA.LEXUS_RX,
"LEXUS RX 2020": TOYOTA.LEXUS_RX_TSS2,
"LEXUS GS F 2016": TOYOTA.LEXUS_GS_F,
"VOLKSWAGEN ARTEON 1ST GEN": VW.VOLKSWAGEN_ARTEON_MK1,
"VOLKSWAGEN ATLAS 1ST GEN": VW.VOLKSWAGEN_ATLAS_MK1,
"VOLKSWAGEN CADDY 3RD GEN": VW.VOLKSWAGEN_CADDY_MK3,
"VOLKSWAGEN CRAFTER 2ND GEN": VW.VOLKSWAGEN_CRAFTER_MK2,
"VOLKSWAGEN GOLF 7TH GEN": VW.VOLKSWAGEN_GOLF_MK7,
"VOLKSWAGEN JETTA 6TH GEN": VW.VOLKSWAGEN_JETTA_MK6,
"VOLKSWAGEN JETTA 7TH GEN": VW.VOLKSWAGEN_JETTA_MK7,
"VOLKSWAGEN PASSAT 7TH GEN": VW.VOLKSWAGEN_PASSAT_MK7,
"VOLKSWAGEN PASSAT 8TH GEN": VW.VOLKSWAGEN_PASSAT_MK8,
"VOLKSWAGEN PASSAT NMS": VW.VOLKSWAGEN_PASSAT_NMS,
"VOLKSWAGEN POLO 6TH GEN": VW.VOLKSWAGEN_POLO_MK6,
"VOLKSWAGEN SHARAN 2ND GEN": VW.VOLKSWAGEN_SHARAN_MK2,
"VOLKSWAGEN TAOS 1ST GEN": VW.VOLKSWAGEN_TAOS_MK1,
"VOLKSWAGEN T-CROSS 1ST GEN": VW.VOLKSWAGEN_TCROSS_MK1,
"VOLKSWAGEN TIGUAN 2ND GEN": VW.VOLKSWAGEN_TIGUAN_MK2,
"VOLKSWAGEN TOURAN 2ND GEN": VW.VOLKSWAGEN_TOURAN_MK2,
"VOLKSWAGEN TRANSPORTER T6.1": VW.VOLKSWAGEN_TRANSPORTER_T61,
"VOLKSWAGEN T-ROC 1ST GEN": VW.VOLKSWAGEN_TROC_MK1,
"AUDI A3 3RD GEN": VW.AUDI_A3_MK3,
"AUDI Q2 1ST GEN": VW.AUDI_Q2_MK1,
"AUDI Q3 2ND GEN": VW.AUDI_Q3_MK2,
"AUDI Q5 1st GEN": VW.AUDI_Q5_MK1,
"Porsche Macan 1st GEN": VW.PORSCHE_MACAN_MK1,
"SEAT ATECA 1ST GEN": VW.SEAT_ATECA_MK1,
"SEAT LEON 3RD GEN": VW.SEAT_ATECA_MK1,
"SEAT_LEON_MK3": VW.SEAT_ATECA_MK1,
"SKODA FABIA 4TH GEN": VW.SKODA_FABIA_MK4,
"SKODA KAMIQ 1ST GEN": VW.SKODA_KAMIQ_MK1,
"SKODA KAROQ 1ST GEN": VW.SKODA_KAROQ_MK1,
"SKODA KODIAQ 1ST GEN": VW.SKODA_KODIAQ_MK1,
"SKODA OCTAVIA 3RD GEN": VW.SKODA_OCTAVIA_MK3,
"SKODA SCALA 1ST GEN": VW.SKODA_KAMIQ_MK1,
"SKODA_SCALA_MK1": VW.SKODA_KAMIQ_MK1,
"SKODA SUPERB 3RD GEN": VW.SKODA_SUPERB_MK3,
"mock": MOCK.MOCK,
}

0
artifacts/package_runtime/iqdbc/car/ford/__init__.py Normal file
View File

203
artifacts/package_runtime/iqdbc/car/ford/carcontroller.py Normal file
View File

@@ -0,0 +1,203 @@
import math
import numpy as np
from iqdbc.can import CANPacker
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs
from iqdbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
from iqdbc.car.ford import fordcan
from iqdbc.car.ford.values import CarControllerParams, FordFlags, CAR
from iqdbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
LongCtrlState = structs.CarControl.Actuators.LongControlState
VisualAlert = structs.CarControl.HUDControl.VisualAlert
# CAN FD limits:
# Limit to average banked road since safety doesn't have the roll
AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation. higher actual roll raises lateral acceleration
MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL) # ~2.4 m/s^2
def anti_overshoot(apply_curvature, apply_curvature_last, v_ego):
diff = 0.1
tau = 5 # 5s smooths over the overshoot
dt = DT_CTRL * CarControllerParams.STEER_STEP
alpha = 1 - np.exp(-dt / tau)
lataccel = apply_curvature * (v_ego ** 2)
last_lataccel = apply_curvature_last * (v_ego ** 2)
last_lataccel = apply_hysteresis(lataccel, last_lataccel, diff)
last_lataccel = alpha * lataccel + (1 - alpha) * last_lataccel
output_curvature = last_lataccel / (max(v_ego, 1) ** 2)
return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature]))
def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP):
# No blending at low speed due to lack of torque wind-up and inaccurate current curvature
if v_ego_raw > 9:
apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
current_curvature + CarControllerParams.CURVATURE_ERROR)
# Curvature rate limit after driver torque limit
apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)
# Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration.
# Safety is not aware of the road roll so we subtract a conservative amount at all times
if CP.flags & FordFlags.CANFD:
# Limit curvature to conservative max lateral acceleration
curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2)
apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit))
return apply_curvature
def apply_creep_compensation(accel: float, v_ego: float) -> float:
creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.])
creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.])
accel -= creep_accel
return float(accel)
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_IQ):
super().__init__(dbc_names, CP, CP_IQ)
self.packer = CANPacker(dbc_names[Bus.pt])
self.CAN = fordcan.CanBus(CP)
self.apply_curvature_last = 0
self.anti_overshoot_curvature_last = 0
self.accel = 0.0
self.gas = 0.0
self.brake_request = False
self.main_on_last = False
self.lkas_enabled_last = False
self.steer_alert_last = False
self.lead_distance_bars_last = None
self.distance_bar_frame = 0
def update(self, CC, CC_IQ, CS, now_nanos):
can_sends = []
actuators = CC.actuators
hud_control = CC.hudControl
main_on = CS.out.cruiseState.available
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
### acc buttons ###
if CC.cruiseControl.cancel:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
# if stock lane centering isn't off, send a button press to toggle it off
# the stock system checks for steering pressed, and eventually disengages cruise control
elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
### lateral control ###
# send steer msg at 20Hz
if (self.frame % CarControllerParams.STEER_STEP) == 0:
# Bronco and some other cars consistently overshoot curv requests
# Apply some deadzone + smoothing convergence to avoid oscillations
if self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14):
self.anti_overshoot_curvature_last = anti_overshoot(actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw)
apply_curvature = self.anti_overshoot_curvature_last
else:
apply_curvature = actuators.curvature
# apply rate limits, curvature error limit, and clip to signal range
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
self.apply_curvature_last = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature,
CS.out.vEgoRaw, 0., CC.latActive, self.CP)
if self.CP.flags & FordFlags.CANFD:
# TODO: extended mode
# Ford uses four individual signals to dictate how to drive to the car. Curvature alone (limited to 0.02m/s^2)
# can actuate the steering for a large portion of any lateral movements. However, in order to get further control on
# steer actuation, the other three signals are necessary. Ford controls vehicles differently than most other makes.
# A detailed explanation on ford control can be found here:
# https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706
mode = 1 if CC.latActive else 0
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter))
else:
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -self.apply_curvature_last, 0.))
# send lka msg at 33Hz
if (self.frame % CarControllerParams.LKA_STEP) == 0:
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
### longitudinal control ###
# send acc msg at 50Hz
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
accel = actuators.accel
gas = accel
if CC.longActive:
# Compensate for engine creep at low speed.
# Either the ABS does not account for engine creep, or the correction is very slow
# TODO: verify this applies to EV/hybrid
accel = apply_creep_compensation(accel, CS.out.vEgo)
# The stock system has been seen rate limiting the brake accel to 5 m/s^3,
# however even 3.5 m/s^3 causes some overshoot with a step response.
accel = max(accel, self.accel - (3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL))
accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
gas = float(np.clip(gas, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
# Both gas and accel are in m/s^2, accel is used solely for braking
if not CC.longActive or gas < CarControllerParams.MIN_GAS:
gas = CarControllerParams.INACTIVE_GAS
# PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits
accel_due_to_pitch = 0.0
if len(CC.orientationNED) == 3:
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
accel_pitch_compensated = accel + accel_due_to_pitch
if accel_pitch_compensated > 0.3 or not CC.longActive:
self.brake_request = False
elif accel_pitch_compensated < 0.0:
self.brake_request = True
stopping = CC.actuators.longControlState == LongCtrlState.stopping
# TODO: look into using the actuators packet to send the desired speed
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
self.accel = accel
self.gas = gas
### ui ###
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
# send lkas ui msg at 1Hz or if ui state changes
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
# send acc ui msg at 5Hz or if ui state changes
if hud_control.leadDistanceBars != self.lead_distance_bars_last:
send_ui = True
self.distance_bar_frame = self.frame
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
show_distance_bars = self.frame - self.distance_bar_frame < 400
can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
fcw_alert, CS.out.cruiseState.standstill, show_distance_bars,
hud_control, CS.acc_tja_status_stock_values))
self.main_on_last = main_on
self.lkas_enabled_last = CC.latActive
self.steer_alert_last = steer_alert
self.lead_distance_bars_last = hud_control.leadDistanceBars
new_actuators = actuators.as_builder()
new_actuators.curvature = self.apply_curvature_last
new_actuators.accel = self.accel
new_actuators.gas = self.gas
self.frame += 1
return new_actuators, can_sends

123
artifacts/package_runtime/iqdbc/car/ford/carstate.py Normal file
View File

@@ -0,0 +1,123 @@
from iqdbc.can import CANDefine, CANParser
from iqdbc.car import Bus, create_button_events, structs
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.ford.fordcan import CanBus
from iqdbc.car.ford.values import DBC, CarControllerParams, FordFlags
from iqdbc.car.interfaces import CarStateBase
ButtonType = structs.CarState.ButtonEvent.Type
GearShifter = structs.CarState.GearShifter
TransmissionType = structs.CarParams.TransmissionType
class CarState(CarStateBase):
def __init__(self, CP, CP_IQ):
CarStateBase.__init__(self, CP, CP_IQ)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"]
self.distance_button = 0
self.lc_button = 0
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
ret = structs.CarState()
ret_iq = structs.IQCarState()
# Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
# The vehicle usually recovers out of this state within a minute of normal driving
ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3
# car speed
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
# gas pedal
ret.gasPressed = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100. > 1e-6
# brake pedal
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
# steering wheel
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
ret.espDisabled = cp.vl["Cluster_Info1_FD1"]["DrvSlipCtlMde_D_Rq"] != 0 # 0 is default mode
if self.CP.flags & FordFlags.CANFD:
# this signal is always 0 on non-CAN FD cars
ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3)
# cruise state
is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else False
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2)
if not self.CP.openpilotLongitudinalControl:
ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1
# gear
if self.CP.transmissionType == TransmissionType.automatic:
gear = self.shifter_values.get(cp.vl["PowertrainData_10"]["TrnRng_D_Rq"])
ret.gearShifter = self.parse_gear_shifter(gear)
elif self.CP.transmissionType == TransmissionType.manual:
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.drive
# safety
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"])
# button presses
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
# TODO: block this going to the camera otherwise it will enable stock TJA
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
prev_distance_button = self.distance_button
prev_lc_button = self.lc_button
self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]
self.lc_button = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
# lock info
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]])
ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2
# blindspot sensors
if self.CP.enableBsm:
cp_bsm = cp_cam if self.CP.flags & FordFlags.CANFD else cp
ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
# Stock steering buttons so that we can passthru blinkers etc.
self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
# Stock values from IPMA so that we can retain some stock functionality
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
ret.buttonEvents = [
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.lc_button, prev_lc_button, {1: ButtonType.lkas}),
]
return ret, ret_iq
@staticmethod
def get_can_parsers(CP, CP_IQ):
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).main),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).camera),
}

223
artifacts/package_runtime/iqdbc/car/ford/fingerprints.py Normal file
View File

@@ -0,0 +1,223 @@
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
from iqdbc.car.structs import CarParams
from iqdbc.car.ford.values import CAR
Ecu = CarParams.Ecu
FW_VERSIONS = {
CAR.FORD_BRONCO_SPORT_MK1: {
(Ecu.eps, 0x730, None): [
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-RF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_ESCAPE_MK4: {
(Ecu.eps, 0x730, None): [
b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-NT\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-SA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LX6C-2D053-SD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LV4T-14F397-GG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_ESCAPE_MK4_5: {
(Ecu.eps, 0x730, None): [
b'PZ11-14D003-EA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'PZ1C-2D053-EJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'PJ6T-14H102-ABL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_EXPLORER_MK6: {
(Ecu.eps, 0x730, None): [
b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'P1MC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-KB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LC5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_EXPEDITION_MK4: {
(Ecu.eps, 0x730, None): [
b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'RL14-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_F_150_MK14: {
(Ecu.eps, 0x730, None): [
b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3V-14D003-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'NL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PL34-2D053-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PL3V-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PL3V-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'ML3T-14H102-ABR\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PJ6T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RJ6T-14H102-ACJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RJ6T-14H102-BBC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_F_150_LIGHTNING_MK1: {
(Ecu.abs, 0x760, None): [
b'PL38-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RL38-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RJ6T-14H102-ACJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RJ6T-14H102-BBC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x730, None): [
b'RL38-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_MUSTANG_MACH_E_MK1: {
(Ecu.eps, 0x730, None): [
b'LJ9C-14D003-AM\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ9C-14D003-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ9C-14D003-FA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ9C-14D003-GA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LJ9C-14D003-HA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'LK9C-2D053-CK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LK9C-2D053-CN\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'ML3T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RJ6T-14H102-BAE\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_FOCUS_MK4: {
(Ecu.eps, 0x730, None): [
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_MAVERICK_MK1: {
(Ecu.eps, 0x730, None): [
b'NZ6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'NZ6C-2D053-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NZ6C-2D053-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ6C-2D053-EE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PZ6C-2D053-EF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_RANGER_MK2: {
(Ecu.eps, 0x730, None): [
b'NB3C-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RB3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
b'PB3C-2D053-ZD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB3C-2D053-ZG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PB3C-2D053-ZJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x764, None): [
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'RJ6T-14H102-BBB\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
}

342
artifacts/package_runtime/iqdbc/car/ford/fordcan.py Normal file
View File

@@ -0,0 +1,342 @@
from iqdbc.car import CanBusBase, structs
HUDControl = structs.CarControl.HUDControl
class CanBus(CanBusBase):
def __init__(self, CP=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
@property
def main(self) -> int:
return self.offset
@property
def radar(self) -> int:
return self.offset + 1
@property
def camera(self) -> int:
return self.offset + 2
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int:
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
checksum = mode + counter
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
checksum += sig_val + (sig_val >> 8)
return 0xFF - (checksum & 0xFF)
def create_lka_msg(packer, CAN: CanBus):
"""
Creates an empty CAN message for the Ford LKA Command.
This command can apply "Lane Keeping Aid" maneuvers, which are subject to the PSCM lockout.
Frequency is 33Hz.
"""
return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
curvature_rate: float):
"""
Creates a CAN message for the Ford TJA/LCA Command.
This command can apply "Lane Centering" maneuvers: continuous lane centering for traffic jam assist and highway
driving. It is not subject to the PSCM lockout.
Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
by the following coefficients:
c0: lateral offset between the vehicle and the centerline (positive is right)
c1: heading angle between the vehicle and the centerline (positive is right)
c2: curvature of the centerline (positive is left)
c3: rate of change of curvature of the centerline
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
angle cannot be easily controlled.
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done
using tools such as Forscan.
Frequency is 20Hz.
"""
values = {
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
"LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
# 3=InterventionRight, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
# Makes no difference with curvature control
"LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
# The stock system always uses comfortable
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
}
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float,
curvature_rate: float, counter: int):
"""
Create a CAN message for the new Ford Lane Centering command.
This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
additional signals for a counter and checksum.
Frequency is 20Hz.
"""
values = {
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
# 3=SafeRampOut, 4-7=NotUsed [0|7]
"LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
"LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
"LatCtlPath_No_Cnt": counter, # [0|15]
"LatCtlPath_No_Cs": 0, # [0|255]
}
# calculate checksum
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1]
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, brake_request, v_ego_kph: float):
"""
Creates a CAN message for the Ford ACC Command.
This command can be used to enable ACC, to set the ACC gas/brake/decel values
and to disable ACC.
Frequency is 50Hz.
"""
values = {
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
# No observed acceleration seen from this signal alone. During stock system operation, it appears to
# be the raw acceleration request (AccPrpl_A_Rq when positive, AccBrkTot_A_Rq when negative)
"AccPrpl_A_Pred": -5.0, # Acceleration request: [-5|5.23] m/s^2
"AccResumEnbl_B_Rq": 1 if long_active else 0,
# No observed acceleration seen from this signal alone
"AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h
# TODO: we may be able to improve braking response by utilizing pre-charging better
# When setting these two bits without AccBrkTot_A_Rq, an initial jerk is observed and car may be able to brake temporarily with AccPrpl_A_Rq
"AccBrkPrchg_B_Rq": 1 if brake_request else 0, # Pre-charge brake request: 0=No, 1=Yes
"AccBrkDecel_B_Rq": 1 if brake_request else 0, # Deceleration request: 0=Inactive, 1=Active
"AccStopStat_B_Rq": 1 if stopping else 0,
}
return packer.make_can_msg("ACCDATA", CAN.main, values)
def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
show_distance_bars: bool, hud_control, stock_values: dict):
"""
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
assist status.
Stock functionality is maintained by passing through unmodified signals.
Frequency is 5Hz.
"""
# Tja_D_Stat
if enabled:
if hud_control.leftLaneDepart:
status = 3 # ActiveInterventionLeft
elif hud_control.rightLaneDepart:
status = 4 # ActiveInterventionRight
else:
status = 2 # Active
elif main_on:
if hud_control.leftLaneDepart:
status = 5 # ActiveWarningLeft
elif hud_control.rightLaneDepart:
status = 6 # ActiveWarningRight
else:
status = 1 # Standby
else:
status = 0 # Off
values = {s: stock_values[s] for s in [
"HaDsply_No_Cs",
"HaDsply_No_Cnt",
"AccStopStat_D_Dsply", # ACC stopped status message
"AccTrgDist2_D_Dsply", # ACC target distance
"AccStopRes_B_Dsply",
"TjaWarn_D_Rq", # TJA warning
"TjaMsgTxt_D_Dsply", # TJA text
"IaccLamp_D_Rq", # iACC status icon
"AccMsgTxt_D2_Rq", # ACC text
"FcwDeny_B_Dsply", # FCW disabled
"FcwMemStat_B_Actl", # FCW enabled setting
"AccTGap_B_Dsply", # ACC time gap display setting
"CadsAlignIncplt_B_Actl",
"AccFllwMde_B_Dsply", # ACC follow mode display setting
"CadsRadrBlck_B_Actl",
"CmbbPostEvnt_B_Dsply", # AEB event status
"AccStopMde_B_Dsply", # ACC stop mode display setting
"FcwMemSens_D_Actl", # FCW sensitivity setting
"FcwMsgTxt_D_Rq", # FCW text
"AccWarn_D_Dsply", # ACC warning
"FcwVisblWarn_B_Rq", # FCW visible alert
"FcwAudioWarn_B_Rq", # FCW audio alert
"AccTGap_D_Dsply", # ACC time gap
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
"FdaMem_B_Stat", # FDA enabled setting
]}
values.update({
"Tja_D_Stat": status, # TJA status
})
if CP.openpilotLongitudinalControl:
values.update({
"AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text
"AccMsgTxt_D2_Rq": 0, # ACC text
"AccTGap_B_Dsply": 1 if show_distance_bars else 0, # Show time gap control UI
"AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator
"AccStopMde_B_Dsply": 1 if standstill else 0,
"AccWarn_D_Dsply": 0, # ACC warning
"AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap
})
# Forwards FCW alert from IPMA
if fcw_alert:
values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert
return packer.make_can_msg("ACCDATA_3", CAN.main, values)
def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control,
stock_values: dict):
"""
Creates a CAN message for the Ford IPC IPMA/LKAS status.
Show the LKAS status with the "driver assist" lines in the IPC.
Stock functionality is maintained by passing through unmodified signals.
Frequency is 1Hz.
"""
# LaActvStats_D_Dsply
# R Intvn Warn Supprs Avail No
# L
# Intvn 24 19 14 9 4
# Warn 23 18 13 8 3
# Supprs 22 17 12 7 2
# Avail 21 16 11 6 1
# No 20 15 10 5 0
#
# TODO: test suppress state
if enabled:
lines = 0 # NoLeft_NoRight
if hud_control.leftLaneDepart:
lines += 4
elif hud_control.leftLaneVisible:
lines += 1
if hud_control.rightLaneDepart:
lines += 20
elif hud_control.rightLaneVisible:
lines += 5
elif main_on:
lines = 0
else:
if hud_control.leftLaneDepart:
lines = 3 # WarnLeft_NoRight
elif hud_control.rightLaneDepart:
lines = 15 # NoLeft_WarnRight
else:
lines = 30 # LA_Off
hands_on_wheel_dsply = 1 if steer_alert else 0
values = {s: stock_values[s] for s in [
"FeatConfigIpmaActl",
"FeatNoIpmaActl",
"PersIndexIpma_D_Actl",
"AhbcRampingV_D_Rq", # AHB ramping
"LaDenyStats_B_Dsply", # LKAS error
"CamraDefog_B_Req", # Windshield heater?
"CamraStats_D_Dsply", # Camera status
"DasAlrtLvl_D_Dsply", # DAS alert level
"DasStats_D_Dsply", # DAS status
"DasWarn_D_Dsply", # DAS warning
"AhbHiBeam_D_Rq", # AHB status
"Passthru_63",
"Passthru_48",
]}
values.update({
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
})
return packer.make_can_msg("IPMA_Data", CAN.main, values)
def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False):
"""
Creates a CAN message for the Ford SCCM buttons/switches.
Includes cruise control buttons, turn lights and more.
Frequency is 10Hz.
"""
values = {s: stock_values[s] for s in [
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
"WiprFront_D_Stat",
"LghtAmb_D_Sns",
"AccButtnGapDecPress",
"AccButtnGapIncPress",
"AslButtnOnOffCnclPress",
"AslButtnOnOffPress",
"LaSwtchPos_D_Stat",
"CcAslButtnCnclResPress",
"CcAslButtnDeny_B_Actl",
"CcAslButtnIndxDecPress",
"CcAslButtnIndxIncPress",
"CcAslButtnOffCnclPress",
"CcAslButtnOnOffCncl",
"CcAslButtnOnPress",
"CcAslButtnResDecPress",
"CcAslButtnResIncPress",
"CcAslButtnSetDecPress",
"CcAslButtnSetIncPress",
"CcAslButtnSetPress",
"CcButtnOffPress",
"CcButtnOnOffCnclPress",
"CcButtnOnOffPress",
"CcButtnOnPress",
"HeadLghtHiFlash_D_Actl",
"HeadLghtHiOn_B_StatAhb",
"AhbStat_B_Dsply",
"AccButtnGapTogglePress",
"WiprFrontSwtch_D_Stat",
"HeadLghtHiCtrl_D_RqAhb",
]}
values.update({
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
})
return packer.make_can_msg("Steering_Data_FD1", bus, values)

100
artifacts/package_runtime/iqdbc/car/ford/interface.py Normal file
View File

@@ -0,0 +1,100 @@
import numpy as np
from iqdbc.car import Bus, get_safety_config, structs
from iqdbc.car.carlog import carlog
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.ford.carcontroller import CarController
from iqdbc.car.ford.carstate import CarState
from iqdbc.car.ford.fordcan import CanBus
from iqdbc.car.ford.radar_interface import RadarInterface
from iqdbc.car.ford.values import CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags
from iqdbc.car.interfaces import CarInterfaceBase
TransmissionType = structs.CarParams.TransmissionType
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
DRIVABLE_GEARS = (structs.CarState.GearShifter.low, structs.CarState.GearShifter.manumatic)
@staticmethod
def get_pid_accel_limits(CP, CP_IQ, current_speed, cruise_speed):
# PCM doesn't allow acceleration near cruise_speed,
# so limit limits of pid to prevent windup
ACCEL_MAX_VALS = [CarControllerParams.ACCEL_MAX, 0.2]
ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .4]
return CarControllerParams.ACCEL_MIN, np.interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "ford"
ret.radarUnavailable = Bus.radar not in DBC[candidate]
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.steerActuatorDelay = 0.2
ret.steerLimitTimer = 1.0
ret.steerAtStandstill = True
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.5]
if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR:
# average of 33.3 Hz radar timestep / 4 scan modes = 60 ms
# MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms
ret.radarDelay = 0.06
CAN = CanBus(fingerprint=fingerprint)
cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)]
if CAN.main >= 4:
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
ret.safetyConfigs = cfgs
ret.alphaLongitudinalAvailable = ret.radarUnavailable
if alpha_long or not ret.radarUnavailable:
ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.LONG_CONTROL.value
ret.openpilotLongitudinalControl = True
if ret.flags & FordFlags.CANFD:
ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.CANFD.value
# TRON (SecOC) platforms are not supported
# LateralMotionControl2, ACCDATA are 16 bytes on these platforms
if len(fingerprint[CAN.camera]):
if fingerprint[CAN.camera].get(0x3d6) != 8 or fingerprint[CAN.camera].get(0x186) != 8:
carlog.error('dashcamOnly: SecOC is unsupported')
ret.dashcamOnly = True
else:
# Lock out if the car does not have needed lateral and longitudinal control APIs.
# Note that we also check CAN for adaptive cruise, but no known signal for LCA exists
pscm_config = next((fw for fw in car_fw if fw.ecu == Ecu.eps and b'\x22\xDE\x01' in fw.request), None)
if pscm_config:
if len(pscm_config.fwVersion) != 24:
carlog.error('dashcamOnly: Invalid EPS FW version')
ret.dashcamOnly = True
else:
config_tja = pscm_config.fwVersion[7] # Traffic Jam Assist
config_lca = pscm_config.fwVersion[8] # Lane Centering Assist
if config_tja != 0xFF or config_lca != 0xFF:
carlog.error('dashcamOnly: Car lacks required lateral control APIs')
ret.dashcamOnly = True
# Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
found_ecus = [fw.ecu for fw in car_fw]
if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs:
ret.transmissionType = TransmissionType.automatic
else:
ret.transmissionType = TransmissionType.manual
ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
# TODO: detect bsm in car_fw?
ret.enableBsm = 0x3A6 in fingerprint[CAN.main] and 0x3A7 in fingerprint[CAN.main]
# LCA can steer down to zero
ret.minSteerSpeed = 0.
ret.autoResumeSng = ret.minEnableSpeed == -1.
ret.centerToFront = ret.wheelbase * 0.44
return ret

272
artifacts/package_runtime/iqdbc/car/ford/radar_interface.py Normal file
View File

@@ -0,0 +1,272 @@
import numpy as np
from typing import cast
from collections import defaultdict
from math import cos, sin
from dataclasses import dataclass
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.ford.fordcan import CanBus
from iqdbc.car.ford.values import DBC, RADAR
from iqdbc.car.interfaces import RadarInterfaceBase
DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
DELPHI_MRR_RADAR_START_ADDR = 0x120
DELPHI_MRR_RADAR_HEADER_ADDR = 0x174 # MRR_Header_SensorCoverage
DELPHI_MRR_RADAR_MSG_COUNT = 64
DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175} # scan index to detection range (m)
DELPHI_MRR_MIN_LONG_RANGE_DIST = 30 # meters
DELPHI_MRR_CLUSTER_THRESHOLD = 5 # meters, lateral distance and relative velocity are weighted
@dataclass
class Cluster:
dRel: float = 0.0
yRel: float = 0.0
vRel: float = 0.0
trackId: int = 0
def cluster_points(pts_l: list[list[float]], pts2_l: list[list[float]], max_dist: float) -> list[int]:
"""
Clusters a collection of points based on another collection of points. This is useful for correlating clusters through time.
Points in pts2 not close enough to any point in pts are assigned -1.
Args:
pts_l: List of points to base the new clusters on
pts2_l: List of points to cluster using pts
max_dist: Max distance from cluster center to candidate point
Returns:
List of cluster indices for pts2 that correspond to pts
"""
if not len(pts2_l):
return []
if not len(pts_l):
return [-1] * len(pts2_l)
max_dist_sq = max_dist ** 2
pts = np.array(pts_l)
pts2 = np.array(pts2_l)
# Compute squared norms
pts_norm_sq = np.sum(pts ** 2, axis=1)
pts2_norm_sq = np.sum(pts2 ** 2, axis=1)
# Compute squared Euclidean distances using the identity
# dist_sq[i, j] = ||pts2[i]||^2 + ||pts[j]||^2 - 2 * pts2[i] . pts[j]
dist_sq = pts2_norm_sq[:, np.newaxis] + pts_norm_sq[np.newaxis, :] - 2 * np.dot(pts2, pts.T)
dist_sq = np.maximum(dist_sq, 0.0)
# Find the closest cluster for each point and assign its index
closest_clusters = np.argmin(dist_sq, axis=1)
closest_dist_sq = dist_sq[np.arange(len(pts2)), closest_clusters]
cluster_idxs = np.where(closest_dist_sq < max_dist_sq, closest_clusters, -1)
return cast(list[int], cluster_idxs.tolist())
def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
msg_n = len(DELPHI_ESR_RADAR_MSGS)
messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n, strict=True))
return CANParser(RADAR.DELPHI_ESR, messages, CanBus(CP).radar)
def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
messages = [
("MRR_Header_InformationDetections", 33),
("MRR_Header_SensorCoverage", 33),
]
for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
msg = f"MRR_Detection_{i:03d}"
messages += [(msg, 33)]
return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP, CP_IQ):
super().__init__(CP, CP_IQ)
self.points: list[list[float]] = []
self.clusters: list[Cluster] = []
self.updated_messages = set()
self.track_id = 0
self.radar = DBC[CP.carFingerprint].get(Bus.radar)
self.scan_index_invalid_cnt = 0
self.radar_unavailable_cnt = 0
self.prev_headerScanIndex = 0
if CP.radarUnavailable:
self.rcp = None
elif self.radar == RADAR.DELPHI_ESR:
self.rcp = _create_delphi_esr_radar_can_parser(CP)
self.trigger_msg = DELPHI_ESR_RADAR_MSGS[-1]
self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS}
elif self.radar == RADAR.DELPHI_MRR:
self.rcp = _create_delphi_mrr_radar_can_parser(CP)
self.trigger_msg = DELPHI_MRR_RADAR_HEADER_ADDR
else:
raise ValueError(f"Unsupported radar: {self.radar}")
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
vls = self.rcp.update(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
self.updated_messages.clear()
ret = structs.RadarData()
if not self.rcp.can_valid:
ret.errors.canError = True
if self.radar == RADAR.DELPHI_ESR:
self._update_delphi_esr()
elif self.radar == RADAR.DELPHI_MRR:
_update = self._update_delphi_mrr(ret)
if not _update:
return None
ret.points = list(self.pts.values())
return ret
def _update_delphi_esr(self):
for ii in sorted(self.updated_messages):
cpt = self.rcp.vl[ii]
if cpt['X_Rel'] > 0.00001:
self.valid_cnt[ii] = 0 # reset counter
if cpt['X_Rel'] > 0.00001:
self.valid_cnt[ii] += 1
else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
#print ii, self.valid_cnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
# radar point only valid if there have been enough valid measurements
if self.valid_cnt[ii] > 0:
if ii not in self.pts:
self.pts[ii] = structs.RadarData.RadarPoint()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['X_Rel'] # from front of car
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['V_Rel']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
def _update_delphi_mrr(self, ret: structs.RadarData):
headerScanIndex = int(self.rcp.vl["MRR_Header_InformationDetections"]['CAN_SCAN_INDEX']) & 0b11
# In reverse, the radar continually sends the last messages. Mark this as invalid
if (self.prev_headerScanIndex + 1) % 4 != headerScanIndex:
self.radar_unavailable_cnt += 1
else:
self.radar_unavailable_cnt = 0
self.prev_headerScanIndex = headerScanIndex
if self.radar_unavailable_cnt >= 5:
self.pts.clear()
self.points.clear()
self.clusters.clear()
ret.errors.radarUnavailableTemporary = True
return True
# Use points with Doppler coverage of +-60 m/s, reduces similar points
if headerScanIndex not in (2, 3):
return False
if DELPHI_MRR_RADAR_RANGE_COVERAGE[headerScanIndex] != int(self.rcp.vl["MRR_Header_SensorCoverage"]["CAN_RANGE_COVERAGE"]):
self.scan_index_invalid_cnt += 1
else:
self.scan_index_invalid_cnt = 0
# Rarely MRR_Header_InformationDetections can fail to send a message. The scan index is skipped in this case
if self.scan_index_invalid_cnt >= 5:
ret.errors.wrongConfig = True
for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
# SCAN_INDEX rotates through 0..3 on each message for different measurement modes
# Indexes 0 and 2 have a max range of ~40m, 1 and 3 are ~170m (MRR_Header_SensorCoverage->CAN_RANGE_COVERAGE)
# Indexes 0 and 1 have a Doppler coverage of +-71 m/s, 2 and 3 have +-60 m/s
scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"]
# Throw out old measurements. Very unlikely to happen, but is proper behavior
if scanIndex != headerScanIndex:
continue
valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
# Long range measurement mode is more sensitive and can detect the road surface
dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984]
if scanIndex in (1, 3) and dist < DELPHI_MRR_MIN_LONG_RANGE_DIST:
valid = False
if valid:
azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964]
distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984]
dRel = cos(azimuth) * dist # m from front of car
yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive
self.points.append([dRel, yRel * 2, distRate * 2])
# Cluster and publish using stored points once we've cycled through all 4 scan modes
if headerScanIndex != 3:
return False
# Cluster points from this cycle against the centroids from the previous cycle
prev_keys = [[p.dRel, p.yRel * 2, p.vRel * 2] for p in self.clusters]
labels = cluster_points(prev_keys, self.points, DELPHI_MRR_CLUSTER_THRESHOLD)
points_by_track_id = defaultdict(list)
for idx, label in enumerate(labels):
if label != -1:
points_by_track_id[self.clusters[label].trackId].append(self.points[idx])
else:
points_by_track_id[self.track_id].append(self.points[idx])
self.track_id += 1
self.clusters = []
for idx, (track_id, pts) in enumerate(points_by_track_id.items()):
dRel = [p[0] for p in pts]
min_dRel = min(dRel)
dRel = sum(dRel) / len(dRel)
yRel = [p[1] for p in pts]
yRel = sum(yRel) / len(yRel) / 2
vRel = [p[2] for p in pts]
vRel = sum(vRel) / len(vRel) / 2
# FIXME: creating capnp RadarPoint and accessing attributes are both expensive, so we store a dataclass and reuse the RadarPoint
self.clusters.append(Cluster(dRel=dRel, yRel=yRel, vRel=vRel, trackId=track_id))
if idx not in self.pts:
self.pts[idx] = structs.RadarData.RadarPoint(measured=True, aRel=float('nan'), yvRel=float('nan'))
self.pts[idx].dRel = min_dRel
self.pts[idx].yRel = yRel
self.pts[idx].vRel = vRel
self.pts[idx].trackId = track_id
for idx in range(len(points_by_track_id), len(self.pts)):
del self.pts[idx]
self.points = []
return True

0
artifacts/package_runtime/iqdbc/car/ford/tests/__init__.py Normal file
View File

28
artifacts/package_runtime/iqdbc/car/ford/tests/print_platform_codes.py Normal file
View File

@@ -0,0 +1,28 @@
#!/usr/bin/env python3
from collections import defaultdict
from iqdbc.car.structs import CarParams
from iqdbc.car.ford.values import get_platform_codes
from iqdbc.car.ford.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
if __name__ == "__main__":
cars_for_code: defaultdict = defaultdict(lambda: defaultdict(set))
for car_model, ecus in FW_VERSIONS.items():
print(car_model)
for ecu in sorted(ecus):
platform_codes = get_platform_codes(ecus[ecu])
for code in platform_codes:
cars_for_code[ecu][code].add(car_model)
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
print(f' Codes: {sorted(platform_codes)}')
print()
print('\nCar models vs. platform codes:')
for ecu, codes in cars_for_code.items():
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
for code, cars in codes.items():
print(f' {code!r}: {sorted(map(str, cars))}')

142
artifacts/package_runtime/iqdbc/car/ford/tests/test_ford.py Normal file
View File

@@ -0,0 +1,142 @@
import random
from collections.abc import Iterable
from hypothesis import settings, given, strategies as st
from parameterized import parameterized
from iqdbc.car.structs import CarParams
from iqdbc.car.fw_versions import build_fw_dict
from iqdbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes
from iqdbc.car.ford.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
ECU_ADDRESSES = {
Ecu.eps: 0x730, # Power Steering Control Module (PSCM)
Ecu.abs: 0x760, # Anti-Lock Brake System (ABS)
Ecu.fwdRadar: 0x764, # Cruise Control Module (CCM)
Ecu.fwdCamera: 0x706, # Image Processing Module A (IPMA)
Ecu.engine: 0x7E0, # Powertrain Control Module (PCM)
Ecu.shiftByWire: 0x732, # Gear Shift Module (GSM)
Ecu.debug: 0x7D0, # Accessory Protocol Interface Module (APIM)
}
ECU_PART_NUMBER = {
Ecu.eps: [
b"14D003",
],
Ecu.abs: [
b"2D053",
],
Ecu.fwdRadar: [
b"14D049",
],
Ecu.fwdCamera: [
b"14F397", # Ford Q3
b"14H102", # Ford Q4
],
}
class TestFordFW:
def test_fw_query_config(self):
for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus:
assert ecu in ECU_ADDRESSES, "Unknown ECU"
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch"
assert subaddr is None, "Unexpected ECU subaddress"
@parameterized.expand(FW_VERSIONS.items())
def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[int, int, int | None], Iterable[bytes]]):
for (ecu, addr, subaddr), fws in fw_versions.items():
assert ecu in ECU_PART_NUMBER, "Unexpected ECU"
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch"
assert subaddr is None, "Unexpected ECU subaddress"
for fw in fws:
assert len(fw) == 24, "Expected ECU response to be 24 bytes"
match = FW_PATTERN.match(fw)
assert match is not None, f"Unable to parse FW: {fw!r}"
if match:
part_number = match.group("part_number")
assert part_number in ECU_PART_NUMBER[ecu], f"Unexpected part number for {fw!r}"
codes = get_platform_codes([fw])
assert 1 == len(codes), f"Unable to parse FW: {fw!r}"
@settings(max_examples=100)
@given(data=st.data())
def test_platform_codes_fuzzy_fw(self, data):
"""Ensure function doesn't raise an exception"""
fw_strategy = st.lists(st.binary())
fws = data.draw(fw_strategy)
get_platform_codes(fws)
def test_platform_codes_spot_check(self):
# Asserts basic platform code parsing behavior for a few cases
results = get_platform_codes([
b"JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00",
b"NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
b"PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00",
b"LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00",
])
assert results == {(b"X6A", b"J"), (b"Z6T", b"N"), (b"J6T", b"P"), (b"B5A", b"L")}
def test_fuzzy_match(self):
for platform, fw_by_addr in FW_VERSIONS.items():
# Ensure there's no overlaps in platform codes
for _ in range(20):
car_fw = []
for ecu, fw_versions in fw_by_addr.items():
ecu_name, addr, sub_addr = ecu
fw = random.choice(fw_versions)
car_fw.append(CarParams.CarFw(ecu=ecu_name, fwVersion=fw, address=addr,
subAddress=0 if sub_addr is None else sub_addr))
CP = CarParams(carFw=car_fw)
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS)
assert matches == {platform}
def test_match_fw_fuzzy(self):
offline_fw = {
(Ecu.eps, 0x730, None): [
b"L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
b"L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
],
(Ecu.abs, 0x760, None): [
b"L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
b"L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
],
(Ecu.fwdRadar, 0x764, None): [
b"LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
b"LB5T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
],
# We consider all model year hints for ECU, even with different platform codes
(Ecu.fwdCamera, 0x706, None): [
b"LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
b"NC5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
],
}
expected_fingerprint = CAR.FORD_EXPLORER_MK6
# ensure that we fuzzy match on all non-exact FW with changed revisions
live_fw = {
(0x730, None): {b"L1MC-14D003-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
(0x760, None): {b"L1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
(0x764, None): {b"LB5T-14D049-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
(0x706, None): {b"LB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
}
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw})
assert candidates == {expected_fingerprint}
# model year hint in between the range should match
live_fw[(0x706, None)] = {b"MB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw,})
assert candidates == {expected_fingerprint}
# unseen model year hint should not match
live_fw[(0x760, None)] = {b"M1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw})
assert len(candidates) == 0, "Should not match new model year hint"

315
artifacts/package_runtime/iqdbc/car/ford/values.py Normal file
View File

@@ -0,0 +1,315 @@
import copy
import re
from dataclasses import dataclass, field, replace
from enum import Enum, IntFlag
from iqdbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from iqdbc.car.lateral import AngleSteeringLimits
from iqdbc.car.structs import CarParams
from iqdbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
from iqdbc.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16
Ecu = CarParams.Ecu
class CarControllerParams:
STEER_STEP = 5 # LateralMotionControl, 20Hz
LKA_STEP = 3 # Lane_Assist_Data1, 33Hz
ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz
LKAS_UI_STEP = 100 # IPMA_Data, 1Hz
ACC_UI_STEP = 20 # ACCDATA_3, 5Hz
BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast
STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
0.02, # Max curvature for steering command, m^-1
# Curvature rate limits
# Max curvature is limited by the EPS to an equivalent of ~2.0 m/s^2 at all speeds,
# however max curvature rate linearly decreases as speed increases:
# ~0.009 m^-1/sec at 7 m/s, ~0.002 m^-1/sec at 35 m/s
# Limit to ~2 m/s^3 up, ~3.3 m/s^3 down at 75 mph and match EPS limit at low speed
([5, 25], [0.00045, 0.0001]),
([5, 25], [0.00045, 0.00015])
)
CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s
ACCEL_MAX = 2.0 # m/s^2 max acceleration
ACCEL_MIN = -3.5 # m/s^2 max deceleration
MIN_GAS = -0.5
INACTIVE_GAS = -5.0
def __init__(self, CP):
pass
class FordSafetyFlags(IntFlag):
LONG_CONTROL = 1
CANFD = 2
class FordFlags(IntFlag):
# Static flags
CANFD = 1
class RADAR:
DELPHI_ESR = 'ford_fusion_2018_adas'
DELPHI_MRR = 'FORD_CADS'
class Footnote(Enum):
FOCUS = CarFootnote(
"Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in " +
"North and South America/Southeast Asia.",
Column.MODEL,
)
@dataclass
class FordCarDocs(CarDocs):
package: str = "Co-Pilot360 Assist+"
hybrid: bool = False
plug_in_hybrid: bool = False
def init_make(self, CP: CarParams):
harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3
self.car_parts = CarParts.common([harness])
if harness == CarHarness.ford_q4:
self.setup_video = "https://www.youtube.com/watch?v=uUGkH6C_EQU"
if CP.carFingerprint in (CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_EXPEDITION_MK4):
self.setup_video = "https://www.youtube.com/watch?v=MewJc9LYp9M"
@dataclass
class FordPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {
Bus.pt: 'ford_lincoln_base_pt',
Bus.radar: RADAR.DELPHI_MRR,
})
def init(self):
for car_docs in list(self.car_docs):
if car_docs.hybrid:
name = f"{car_docs.make} {car_docs.model} Hybrid {car_docs.years}"
self.car_docs.append(replace(copy.deepcopy(car_docs), name=name))
if car_docs.plug_in_hybrid:
name = f"{car_docs.make} {car_docs.model} Plug-in Hybrid {car_docs.years}"
self.car_docs.append(replace(copy.deepcopy(car_docs), name=name))
@dataclass
class FordCANFDPlatformConfig(FordPlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {
Bus.pt: 'ford_lincoln_base_pt',
})
def init(self):
super().init()
self.flags |= FordFlags.CANFD
@dataclass
class FordF150LightningPlatform(FordCANFDPlatformConfig):
def init(self):
super().init()
# Don't show in docs until this issue is resolved. See https://github.com/commaai/openpilot/issues/30302
self.car_docs = []
class CAR(Platforms):
FORD_BRONCO_SPORT_MK1 = FordPlatformConfig(
[FordCarDocs("Ford Bronco Sport 2021-24")],
CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7),
)
FORD_ESCAPE_MK4 = FordPlatformConfig(
[
FordCarDocs("Ford Escape 2020-22", hybrid=True, plug_in_hybrid=True),
FordCarDocs("Ford Kuga 2020-23", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True),
],
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
)
FORD_ESCAPE_MK4_5 = FordCANFDPlatformConfig(
[
FordCarDocs("Ford Escape 2023-24", hybrid=True, plug_in_hybrid=True, setup_video="https://www.youtube.com/watch?v=M6uXf4b2SHM"),
FordCarDocs("Ford Kuga Hybrid 2024", "All"),
FordCarDocs("Ford Kuga Plug-in Hybrid 2024", "All"),
],
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
)
FORD_EXPLORER_MK6 = FordPlatformConfig(
[
FordCarDocs("Ford Explorer 2020-24", hybrid=True), # Hybrid: Limited and Platinum only
FordCarDocs("Lincoln Aviator 2020-24", "Co-Pilot360 Plus", plug_in_hybrid=True), # Hybrid: Grand Touring only
],
CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8),
)
FORD_EXPEDITION_MK4 = FordCANFDPlatformConfig(
[FordCarDocs("Ford Expedition 2022-24", "Co-Pilot360 Assist 2.0", hybrid=False)],
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
)
FORD_F_150_MK14 = FordCANFDPlatformConfig(
[FordCarDocs("Ford F-150 2021-23", "Co-Pilot360 Assist 2.0", hybrid=True)],
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
)
FORD_F_150_LIGHTNING_MK1 = FordF150LightningPlatform(
[FordCarDocs("Ford F-150 Lightning 2022-23", "Co-Pilot360 Assist 2.0")],
CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9),
)
FORD_FOCUS_MK4 = FordPlatformConfig(
[FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only
CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0),
)
FORD_MAVERICK_MK1 = FordPlatformConfig(
[
FordCarDocs("Ford Maverick 2022", "LARIAT Luxury", hybrid=True),
FordCarDocs("Ford Maverick 2023-24", "Co-Pilot360 Assist", hybrid=True),
],
CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0),
)
FORD_MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig(
[FordCarDocs("Ford Mustang Mach-E 2021-24", "All", setup_video="https://www.youtube.com/watch?v=AR4_eTF3b_A")],
CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0), # TODO: check steer ratio
)
FORD_RANGER_MK2 = FordCANFDPlatformConfig(
[FordCarDocs("Ford Ranger 2024", "Adaptive Cruise Control with Lane Centering", setup_video="https://www.youtube.com/watch?v=2oJlXCKYOy0")],
CarSpecs(mass=2000, wheelbase=3.27, steerRatio=17.0),
)
# FW response contains a combined software and part number
# A-Z except no I, O or W
# e.g. NZ6A-14C204-AAA
# 1222-333333-444
# 1 = Model year hint (approximates model year/generation)
# 2 = Platform hint
# 3 = Part number
# 4 = Software version
FW_ALPHABET = b'A-HJ-NP-VX-Z'
FW_PATTERN = re.compile(b'^(?P<model_year_hint>[' + FW_ALPHABET + b'])' +
b'(?P<platform_hint>[0-9' + FW_ALPHABET + b']{3})-' +
b'(?P<part_number>[0-9' + FW_ALPHABET + b']{5,6})-' +
b'(?P<software_revision>[' + FW_ALPHABET + b']{2,})\x00*$')
def get_platform_codes(fw_versions: list[bytes] | set[bytes]) -> set[tuple[bytes, bytes]]:
codes = set()
for fw in fw_versions:
match = FW_PATTERN.match(fw)
if match is not None:
codes.add((match.group('platform_hint'), match.group('model_year_hint')))
return codes
def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw_versions: OfflineFwVersions) -> set[str]:
candidates: set[str] = set()
for candidate, fws in offline_fw_versions.items():
# Keep track of ECUs which pass all checks (platform hint, within model year hint range)
valid_found_ecus = set()
valid_expected_ecus = {ecu[1:] for ecu in fws if ecu[0] in PLATFORM_CODE_ECUS}
for ecu, expected_versions in fws.items():
addr = ecu[1:]
# Only check ECUs expected to have platform codes
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
# Expected platform codes & model year hints
codes = get_platform_codes(expected_versions)
expected_platform_codes = {code for code, _ in codes}
expected_model_year_hints = {model_year_hint for _, model_year_hint in codes}
# Found platform codes & model year hints
codes = get_platform_codes(live_fw_versions.get(addr, set()))
found_platform_codes = {code for code, _ in codes}
found_model_year_hints = {model_year_hint for _, model_year_hint in codes}
# Check platform code matches for any found versions
if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes):
break
# Check any model year hint within range in the database. Note that some models have more than one
# platform code per ECU which we don't consider as separate ranges
if not any(min(expected_model_year_hints) <= found_model_year_hint <= max(expected_model_year_hints) for
found_model_year_hint in found_model_year_hints):
break
valid_found_ecus.add(addr)
# If all live ECUs pass all checks for candidate, add it as a match
if valid_expected_ecus.issubset(valid_found_ecus):
candidates.add(candidate)
return candidates
# All of these ECUs must be present and are expected to have platform codes we can match
PLATFORM_CODE_ECUS = (Ecu.abs, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps)
DATA_IDENTIFIER_FORD_ASBUILT = 0xDE00
ASBUILT_BLOCKS: list[tuple[int, list]] = [
(1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps]),
(2, [Ecu.abs, Ecu.debug, Ecu.eps]),
(3, [Ecu.abs, Ecu.debug, Ecu.eps]),
(4, [Ecu.debug, Ecu.fwdCamera]),
(5, [Ecu.debug]),
(6, [Ecu.debug]),
(7, [Ecu.debug]),
(8, [Ecu.debug]),
(9, [Ecu.debug]),
(16, [Ecu.debug, Ecu.fwdCamera]),
(18, [Ecu.fwdCamera]),
(20, [Ecu.fwdCamera]),
(21, [Ecu.fwdCamera]),
]
def ford_asbuilt_block_request(block_id: int):
return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1)
def ford_asbuilt_block_response(block_id: int):
return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1)
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
# CAN and CAN FD queries are combined.
# FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire],
logging=True,
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire],
bus=0,
auxiliary=True,
),
*[Request(
[StdQueries.TESTER_PRESENT_REQUEST, ford_asbuilt_block_request(block_id)],
[StdQueries.TESTER_PRESENT_RESPONSE, ford_asbuilt_block_response(block_id)],
whitelist_ecus=ecus,
bus=0,
logging=True,
) for block_id, ecus in ASBUILT_BLOCKS],
],
extra_ecus=[
(Ecu.engine, 0x7e0, None), # Powertrain Control Module
# Note: We are unlikely to get a response from behind the gateway
(Ecu.shiftByWire, 0x732, None), # Gear Shift Module
(Ecu.debug, 0x7d0, None), # Accessory Protocol Interface Module
],
# Custom fuzzy fingerprinting function using platform and model year hints
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
DBC = CAR.create_dbc_map()

153
artifacts/package_runtime/iqdbc/car/fw_query_definitions.py Normal file
View File

@@ -0,0 +1,153 @@
import copy
from dataclasses import dataclass, field
import struct
from collections.abc import Callable
from iqdbc.car import uds
from iqdbc.car.structs import CarParams
Ecu = CarParams.Ecu
AddrType = tuple[int, int | None]
EcuAddrBusType = tuple[int, int | None, int]
EcuAddrSubAddr = tuple[Ecu, int, int | None]
LiveFwVersions = dict[AddrType, set[bytes]]
OfflineFwVersions = dict[str, dict[EcuAddrSubAddr, list[bytes]]]
# A global list of addresses we will only ever consider for VIN responses
# engine, hybrid controller, Ford abs, Hyundai CAN FD cluster, 29-bit engine, PGM-FI
# TODO: move these to each brand's FW query config
STANDARD_VIN_ADDRS = [0x7e0, 0x7e2, 0x760, 0x7c6, 0x18da10f1, 0x18da0ef1]
ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()}
def p16(val):
return struct.pack("!H", val)
class StdQueries:
# FW queries
TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0])
TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0])
SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT])
SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40])
DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
uds.SESSION_TYPE.DEFAULT])
DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4])
EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC])
EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4])
MANUFACTURER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
SUPPLIER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER)
SUPPLIER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER)
MANUFACTURER_ECU_HARDWARE_NUMBER_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_HARDWARE_NUMBER)
MANUFACTURER_ECU_HARDWARE_NUMBER_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_HARDWARE_NUMBER)
UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
OBD_VERSION_REQUEST = b'\x09\x04'
OBD_VERSION_RESPONSE = b'\x49\x04'
# VIN queries
OBD_VIN_REQUEST = b'\x09\x02'
OBD_VIN_RESPONSE = b'\x49\x02\x01'
UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN)
UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN)
GM_VIN_REQUEST = b'\x1a\x90'
GM_VIN_RESPONSE = b'\x5a\x90'
KWP_VIN_REQUEST = b'\x21\x81'
KWP_VIN_RESPONSE = b'\x61\x81'
@dataclass
class Request:
request: list[bytes]
response: list[bytes]
whitelist_ecus: list[Ecu] = field(default_factory=list)
rx_offset: int = 0x8
bus: int = 1
# Whether this query should be run on the first auxiliary panda (CAN FD cars for example)
auxiliary: bool = False
# FW responses from these queries will not be used for fingerprinting
logging: bool = False
# pandad toggles OBD multiplexing on/off as needed
obd_multiplexing: bool = True
@dataclass
class FwQueryConfig:
requests: list[Request]
# TODO: make this automatic and remove hardcoded lists, or do fingerprinting with ecus
# Overrides and removes from essential ecus for specific models and ecus (exact matching)
non_essential_ecus: dict[Ecu, list[str]] = field(default_factory=dict)
# Ecus added for data collection, not to be fingerprinted on
extra_ecus: list[tuple[Ecu, int, int | None]] = field(default_factory=list)
# Function a brand can implement to provide better fuzzy matching. Takes in FW versions and VIN,
# returns set of candidates. Only will match if one candidate is returned
match_fw_to_car_fuzzy: Callable[[LiveFwVersions, str, OfflineFwVersions], set[str]] | None = None
refine_fw_matches: Callable[[set[str], str], set[str]] | None = None
def __post_init__(self):
# Asserts that a request exists if extra ecus are used
if len(self.extra_ecus):
assert len(self.requests), "Must define a request with extra ecus"
# All extra ecus should be used in a request
for ecu, _, _ in self.extra_ecus:
assert (any(ecu in request.whitelist_ecus for request in self.requests) or
any(not request.whitelist_ecus for request in self.requests)), f"Ecu.{ECU_NAME[ecu]} not in any request"
# These ECUs are already not in ESSENTIAL_ECUS which the fingerprint functions give a pass if missing
unnecessary_non_essential_ecus = set(self.non_essential_ecus) - set(ESSENTIAL_ECUS)
assert unnecessary_non_essential_ecus == set(), ("Declaring non-essential ECUs non-essential is not required: " +
f"{', '.join([f'Ecu.{ECU_NAME[ecu]}' for ecu in unnecessary_non_essential_ecus])}")
# Asserts equal length request and response lists
for request_obj in self.requests:
assert len(request_obj.request) == len(request_obj.response), ("Request and response lengths do not match: " +
f"{request_obj.request} vs. {request_obj.response}")
# No request on the OBD port (bus 1, multiplexed) should be run on an aux panda
assert not (request_obj.auxiliary and request_obj.bus == 1 and request_obj.obd_multiplexing), ("OBD multiplexed request should not " +
f"be marked auxiliary: {request_obj}")
# Add aux requests (second panda) for all requests that are marked as auxiliary
for i in range(len(self.requests)):
if self.requests[i].auxiliary:
new_request = copy.deepcopy(self.requests[i])
new_request.bus += 4
self.requests.append(new_request)
def get_all_ecus(self, offline_fw_versions: OfflineFwVersions,
include_extra_ecus: bool = True) -> set[EcuAddrSubAddr]:
# Add ecus in database + extra ecus
brand_ecus = {ecu for ecus in offline_fw_versions.values() for ecu in ecus}
if include_extra_ecus:
brand_ecus |= set(self.extra_ecus)
return brand_ecus

331
artifacts/package_runtime/iqdbc/car/fw_versions.py Normal file
View File

@@ -0,0 +1,331 @@
from collections import defaultdict
from collections.abc import Callable, Iterator
from typing import Protocol, TypeVar
from tqdm import tqdm
from iqdbc.car import uds
from iqdbc.car.can_definitions import CanRecvCallable, CanSendCallable
from iqdbc.car.carlog import carlog
from iqdbc.car.structs import CarParams
from iqdbc.car.ecu_addrs import get_ecu_addrs
from iqdbc.car.fingerprints import FW_VERSIONS
from iqdbc.car.fw_query_definitions import ESSENTIAL_ECUS, AddrType, EcuAddrBusType, FwQueryConfig, LiveFwVersions, OfflineFwVersions
from iqdbc.car.interfaces import get_interface_attr
from iqdbc.car.isotp_parallel_query import IsoTpParallelQuery
Ecu = CarParams.Ecu
FUZZY_EXCLUDE_ECUS = [Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps, Ecu.debug]
FW_QUERY_CONFIGS: dict[str, FwQueryConfig] = get_interface_attr('FW_QUERY_CONFIG', ignore_none=True)
VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True)
MODEL_TO_BRAND = {c: b for b, e in VERSIONS.items() for c in e}
REQUESTS = [(brand, config, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests]
T = TypeVar('T')
ObdCallback = Callable[[bool], None]
def chunks(l: list[T], n: int = 128) -> Iterator[list[T]]:
for i in range(0, len(l), n):
yield l[i:i + n]
def is_brand(brand: str, filter_brand: str | None) -> bool:
"""Returns if brand matches filter_brand or no brand filter is specified"""
return filter_brand is None or brand == filter_brand
def build_fw_dict(fw_versions: list[CarParams.CarFw], filter_brand: str | None = None) -> dict[AddrType, set[bytes]]:
fw_versions_dict: defaultdict[AddrType, set[bytes]] = defaultdict(set)
for fw in fw_versions:
if is_brand(fw.brand, filter_brand) and not fw.logging:
sub_addr = fw.subAddress if fw.subAddress != 0 else None
fw_versions_dict[(fw.address, sub_addr)].add(fw.fwVersion)
return dict(fw_versions_dict)
class MatchFwToCar(Protocol):
def __call__(self, live_fw_versions: LiveFwVersions, match_brand: str | None = None, log: bool = True) -> set[str]:
...
def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, match_brand: str | None = None, log: bool = True, exclude: str | None = None) -> set[str]:
"""Do a fuzzy FW match. This function will return a match, and the number of firmware version
that were matched uniquely to that specific car. If multiple ECUs uniquely match to different cars
the match is rejected."""
# Build lookup table from (addr, sub_addr, fw) to list of candidate cars
all_fw_versions = defaultdict(list)
for candidate, fw_by_addr in FW_VERSIONS.items():
if not is_brand(MODEL_TO_BRAND[candidate], match_brand):
continue
if candidate == exclude:
continue
for addr, fws in fw_by_addr.items():
# These ECUs are known to be shared between models (EPS only between hybrid/ICE version)
# Getting this exactly right isn't crucial, but excluding camera and radar makes it almost
# impossible to get 3 matching versions, even if two models with shared parts are released at the same
# time and only one is in our database.
if addr[0] in FUZZY_EXCLUDE_ECUS:
continue
for f in fws:
all_fw_versions[(addr[1], addr[2], f)].append(candidate)
matched_ecus = set()
match: str | None = None
for addr, versions in live_fw_versions.items():
ecu_key = (addr[0], addr[1])
for version in versions:
# All cars that have this FW response on the specified address
candidates = all_fw_versions[(*ecu_key, version)]
if len(candidates) == 1:
matched_ecus.add(ecu_key)
if match is None:
match = candidates[0]
# We uniquely matched two different cars. No fuzzy match possible
elif match != candidates[0]:
return set()
# Note that it is possible to match to a candidate without all its ECUs being present
# if there are enough matches. FIXME: parameterize this or require all ECUs to exist like exact matching
if match and len(matched_ecus) >= 2:
if log:
carlog.error(f"Fingerprinted {match} using fuzzy match. {len(matched_ecus)} matching ECUs")
return {match}
else:
return set()
def match_fw_to_car_exact(live_fw_versions: LiveFwVersions, match_brand: str | None = None,
log: bool = True, extra_fw_versions: dict | None = None) -> set[str]:
"""Do an exact FW match. Returns all cars that match the given
FW versions for a list of "essential" ECUs. If an ECU is not considered
essential the FW version can be missing to get a fingerprint, but if it's present it
needs to match the database."""
if extra_fw_versions is None:
extra_fw_versions = {}
invalid = set()
candidates = {c: f for c, f in FW_VERSIONS.items() if
is_brand(MODEL_TO_BRAND[c], match_brand)}
for candidate, fws in candidates.items():
config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]]
for ecu, expected_versions in fws.items():
expected_versions = expected_versions + extra_fw_versions.get(candidate, {}).get(ecu, [])
ecu_type = ecu[0]
addr = ecu[1:]
found_versions = live_fw_versions.get(addr, set())
if not len(found_versions):
# Some models can sometimes miss an ecu, or show on two different addresses
# FIXME: this logic can be improved to be more specific, should require one of the two addresses
if candidate in config.non_essential_ecus.get(ecu_type, []):
continue
# Ignore non essential ecus
if ecu_type not in ESSENTIAL_ECUS:
continue
# Virtual debug ecu doesn't need to match the database
if ecu_type == Ecu.debug:
continue
if not any(found_version in expected_versions for found_version in found_versions):
invalid.add(candidate)
break
return set(candidates.keys()) - invalid
def match_fw_to_car(fw_versions: list[CarParams.CarFw], vin: str, allow_exact: bool = True,
allow_fuzzy: bool = True, log: bool = True) -> tuple[bool, set[str]]:
# Try exact matching first
exact_matches: list[tuple[bool, MatchFwToCar]] = []
if allow_exact:
exact_matches = [(True, match_fw_to_car_exact)]
if allow_fuzzy:
exact_matches.append((False, match_fw_to_car_fuzzy))
for exact_match, match_func in exact_matches:
# For each brand, attempt to fingerprint using all FW returned from its queries
matches: set[str] = set()
for brand in VERSIONS.keys():
fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand)
config = FW_QUERY_CONFIGS[brand]
brand_matches = match_func(fw_versions_dict, match_brand=brand, log=log)
if exact_match and len(brand_matches) > 1 and config.refine_fw_matches is not None:
brand_matches = config.refine_fw_matches(brand_matches, vin)
matches |= brand_matches
# If specified and no matches so far, fall back to brand's fuzzy fingerprinting function
if not exact_match and not len(matches) and config.match_fw_to_car_fuzzy is not None:
matches |= config.match_fw_to_car_fuzzy(fw_versions_dict, vin, VERSIONS[brand])
if len(matches):
return exact_match, matches
return True, set()
def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int = 1) -> set[EcuAddrBusType]:
# queries are split by OBD multiplexing mode
queries: dict[bool, list[list[EcuAddrBusType]]] = {True: [], False: []}
parallel_queries: dict[bool, list[EcuAddrBusType]] = {True: [], False: []}
responses: set[EcuAddrBusType] = set()
for brand, config, r in REQUESTS:
# Skip query if no panda available
if r.bus > num_pandas * 4 - 1:
continue
for ecu_type, addr, sub_addr in config.get_all_ecus(VERSIONS[brand]):
# Only query ecus in whitelist if whitelist is not empty
if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus:
a = (addr, sub_addr, r.bus)
# Build set of queries
if sub_addr is None:
if a not in parallel_queries[r.obd_multiplexing]:
parallel_queries[r.obd_multiplexing].append(a)
else: # subaddresses must be queried one by one
if [a] not in queries[r.obd_multiplexing]:
queries[r.obd_multiplexing].append([a])
# Build set of expected responses to filter
response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset)
responses.add((response_addr, sub_addr, r.bus))
for obd_multiplexing in queries:
queries[obd_multiplexing].insert(0, parallel_queries[obd_multiplexing])
ecu_responses = set()
for obd_multiplexing in queries:
set_obd_multiplexing(obd_multiplexing)
for query in queries[obd_multiplexing]:
ecu_responses.update(get_ecu_addrs(can_recv, can_send, set(query), responses, timeout=0.1))
return ecu_responses
def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, list[bool]]:
"""Returns dictionary of brands and matches with ECUs in their FW versions"""
brand_rx_addrs = {brand: set() for brand in FW_QUERY_CONFIGS}
brand_matches = {brand: [] for brand, _, _ in REQUESTS}
# Since we can't know what request an ecu responded to, add matches for all possible rx offsets
for brand, config, r in REQUESTS:
for ecu in config.get_all_ecus(VERSIONS[brand]):
if len(r.whitelist_ecus) == 0 or ecu[0] in r.whitelist_ecus:
brand_rx_addrs[brand].add((uds.get_rx_addr_for_tx_addr(ecu[1], r.rx_offset), ecu[2]))
for brand, addrs in brand_rx_addrs.items():
for addr in addrs:
# TODO: check bus from request as well
brand_matches[brand].append(addr in [addr[:2] for addr in ecu_rx_addrs])
return brand_matches
def get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, vin: str,
ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, progress: bool = False) -> list[CarParams.CarFw]:
"""Queries for FW versions ordering brands by likelihood, breaks when exact match is found"""
all_car_fw = []
brand_matches = get_brand_ecu_matches(ecu_rx_addrs)
# Sort brands by number of matching ECUs first, then percentage of matching ECUs in the database
# This allows brands with only one ECU to be queried first (e.g. Tesla)
for brand in sorted(brand_matches, key=lambda b: (brand_matches[b].count(True), brand_matches[b].count(True) / len(brand_matches[b])), reverse=True):
# Skip this brand if there are no matching present ECUs
if True not in brand_matches[brand]:
continue
car_fw = get_fw_versions(can_recv, can_send, set_obd_multiplexing, query_brand=brand, timeout=timeout, num_pandas=num_pandas, progress=progress)
all_car_fw.extend(car_fw)
# If there is a match using this brand's FW alone, finish querying early
_, matches = match_fw_to_car(car_fw, vin, log=False)
if len(matches) == 1:
break
return all_car_fw
def get_fw_versions(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, query_brand: str | None = None,
extra: OfflineFwVersions | None = None, timeout: float = 0.1, num_pandas: int = 1, progress: bool = False) -> list[CarParams.CarFw]:
versions = VERSIONS.copy()
if query_brand is not None:
versions = {query_brand: versions[query_brand]}
if extra is not None:
versions.update(extra)
# Extract ECU addresses to query from fingerprints
# ECUs using a subaddress need be queried one by one, the rest can be done in parallel
addrs = []
parallel_addrs = []
ecu_types = {}
for brand, brand_versions in versions.items():
config = FW_QUERY_CONFIGS[brand]
for ecu_type, addr, sub_addr in config.get_all_ecus(brand_versions):
a = (brand, addr, sub_addr)
if a not in ecu_types:
ecu_types[a] = ecu_type
if sub_addr is None:
if a not in parallel_addrs:
parallel_addrs.append(a)
else:
if [a] not in addrs:
addrs.append([a])
addrs.insert(0, parallel_addrs)
# Get versions and build capnp list to put into CarParams
car_fw = []
requests = [(brand, config, r) for brand, config, r in REQUESTS if is_brand(brand, query_brand)]
for addr_group in tqdm(addrs, disable=not progress): # split by subaddr, if any
for addr_chunk in chunks(addr_group):
for brand, config, r in requests:
# Skip query if no panda available
if r.bus > num_pandas * 4 - 1:
continue
# Toggle OBD multiplexing for each request
if r.bus % 4 == 1:
set_obd_multiplexing(r.obd_multiplexing)
try:
query_addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and
(len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)]
if query_addrs:
query = IsoTpParallelQuery(can_send, can_recv, r.bus, query_addrs, r.request, r.response, r.rx_offset)
for (tx_addr, sub_addr), version in query.get_data(timeout).items():
f = CarParams.CarFw()
f.ecu = ecu_types.get((brand, tx_addr, sub_addr), Ecu.unknown)
f.fwVersion = version
f.address = tx_addr
f.responseAddress = uds.get_rx_addr_for_tx_addr(tx_addr, r.rx_offset)
f.request = r.request
f.brand = brand
f.bus = r.bus
f.logging = r.logging or (f.ecu, tx_addr, sub_addr) in config.extra_ecus
f.obdMultiplexing = r.obd_multiplexing
if sub_addr is not None:
f.subAddress = sub_addr
car_fw.append(f)
except Exception:
carlog.exception("FW query exception")
return car_fw

0
artifacts/package_runtime/iqdbc/car/gm/__init__.py Normal file
View File

163
artifacts/package_runtime/iqdbc/car/gm/carcontroller.py Normal file
View File

@@ -0,0 +1,163 @@
import numpy as np
from iqdbc.can import CANPacker
from iqdbc.car import Bus, DT_CTRL, structs
from iqdbc.car.lateral import apply_driver_steer_torque_limits
from iqdbc.car.gm import gmcan
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons
from iqdbc.car.interfaces import CarControllerBase
VisualAlert = structs.CarControl.HUDControl.VisualAlert
NetworkLocation = structs.CarParams.NetworkLocation
LongCtrlState = structs.CarControl.Actuators.LongControlState
# Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
CAMERA_CANCEL_DELAY_FRAMES = 10
# Enforce a minimum interval between steering messages to avoid a fault
MIN_STEER_MSG_INTERVAL_MS = 15
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_IQ):
super().__init__(dbc_names, CP, CP_IQ)
self.start_time = 0.
self.apply_torque_last = 0
self.apply_gas = 0
self.apply_brake = 0
self.last_steer_frame = 0
self.last_button_frame = 0
self.cancel_counter = 0
self.lka_steering_cmd_counter = 0
self.lka_icon_status_last = (False, False)
self.params = CarControllerParams(self.CP)
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint][Bus.pt])
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint][Bus.radar])
self.packer_ch = CANPacker(DBC[self.CP.carFingerprint][Bus.chassis])
def update(self, CC, CC_IQ, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
hud_alert = hud_control.visualAlert
hud_v_cruise = hud_control.setSpeed
if hud_v_cruise > 70:
hud_v_cruise = 0
# Send CAN commands.
can_sends = []
# Steering (Active: 50Hz, inactive: 10Hz)
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
if self.CP.networkLocation == NetworkLocation.fwdCamera:
# Also send at 50Hz:
# - on startup, first few msgs are blocked
# - until we're in sync with camera so counters align when relay closes, preventing a fault.
# openpilot can subtly drift, so this is activated throughout a drive to stay synced
out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.cam_lka_steering_cmd_counter + 1) % 4
if CS.loopback_lka_steering_cmd_ts_nanos == 0 or out_of_sync:
steer_step = self.params.STEER_STEP
self.lka_steering_cmd_counter += 1 if CS.loopback_lka_steering_cmd_updated else 0
# Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we
# received the ASCMLKASteeringCmd loopback confirmation too recently
last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6
if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS:
# Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus
if CS.loopback_lka_steering_cmd_ts_nanos == 0:
self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1
if CC.latActive:
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.params)
else:
apply_torque = 0
self.last_steer_frame = self.frame
self.apply_torque_last = apply_torque
idx = self.lka_steering_cmd_counter % 4
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_torque, idx, CC.latActive))
if self.CP.openpilotLongitudinalControl:
# Gas/regen, brakes, and UI commands - all at 25Hz
if self.frame % 4 == 0:
stopping = actuators.longControlState == LongCtrlState.stopping
if not CC.longActive:
# ASCM sends max regen when not enabled
self.apply_gas = self.params.INACTIVE_REGEN
self.apply_brake = 0
else:
self.apply_gas = float(np.interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))
self.apply_brake = int(round(np.interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
# Don't allow any gas above inactive regen while stopping
# FIXME: brakes aren't applied immediately when enabling at a stop
if stopping:
self.apply_gas = self.params.INACTIVE_REGEN
idx = (self.frame // 4) % 4
at_full_stop = CC.longActive and CS.out.standstill
near_stop = CC.longActive and (abs(CS.out.vEgo) < self.params.NEAR_STOP_BRAKE_PHASE)
friction_brake_bus = CanBus.CHASSIS
# GM Camera exceptions
# TODO: can we always check the longControlState?
if self.CP.networkLocation == NetworkLocation.fwdCamera:
at_full_stop = at_full_stop and stopping
friction_brake_bus = CanBus.POWERTRAIN
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop))
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,
idx, CC.enabled, near_stop, at_full_stop, self.CP))
# Send dashboard UI commands (ACC status)
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
if not self.CP.radarUnavailable:
tt = self.frame * DT_CTRL
time_and_headlights_step = 10
if self.frame % time_and_headlights_step == 0:
idx = (self.frame // time_and_headlights_step) % 4
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if self.frame % speed_and_accelerometer_step == 0:
idx = (self.frame // speed_and_accelerometer_step) % 4
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, abs(CS.out.vEgo), idx))
if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
else:
# While car is braking, cancel button causes ECM to enter a soft disable state with a fault status.
# A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
# Stock longitudinal, integrated at camera
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
self.last_button_frame = self.frame
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
if self.CP.networkLocation == NetworkLocation.fwdCamera:
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
if self.frame % 10 == 0:
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
new_actuators = actuators.as_builder()
new_actuators.torque = self.apply_torque_last / self.params.STEER_MAX
new_actuators.torqueOutputCan = self.apply_torque_last
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake
self.frame += 1
return new_actuators, can_sends

186
artifacts/package_runtime/iqdbc/car/gm/carstate.py Normal file
View File

@@ -0,0 +1,186 @@
import copy
from iqdbc.can import CANDefine, CANParser
from iqdbc.car import Bus, create_button_events, structs
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.interfaces import CarStateBase
from iqdbc.car.gm.values import DBC, AccState, CruiseButtons, STEER_THRESHOLD, SDGM_CAR, ALT_ACCS
from iqdbc.lvbs.car.gm.iq_carstate import IQCarState
from iqdbc.lvbs.car.gm.iq_values import GMFlagsIQ
ButtonType = structs.CarState.ButtonEvent.Type
TransmissionType = structs.CarParams.TransmissionType
NetworkLocation = structs.CarParams.NetworkLocation
STANDSTILL_THRESHOLD = 10 * 0.0311
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel}
class CarState(CarStateBase, IQCarState):
def __init__(self, CP, CP_IQ):
CarStateBase.__init__(self, CP, CP_IQ)
IQCarState.__init__(self, CP, CP_IQ)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"]
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
self.cluster_min_speed = CV.KPH_TO_MS / 2.
self.loopback_lka_steering_cmd_updated = False
self.loopback_lka_steering_cmd_ts_nanos = 0
self.pt_lka_steering_cmd_counter = 0
self.cam_lka_steering_cmd_counter = 0
self.buttons_counter = 0
self.distance_button = 0
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
if not self.CP.pcmCruise:
for b in buttonEvents:
# The ECM allows enabling on falling edge of set, but only rising edge of resume
if (b.type == ButtonType.accelCruise and b.pressed) or \
(b.type == ButtonType.decelCruise and not b.pressed):
return True
return False
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
pt_cp = can_parsers[Bus.pt]
cam_cp = can_parsers[Bus.cam]
loopback_cp = can_parsers[Bus.loopback]
ret = structs.CarState()
ret_iq = structs.IQCarState()
prev_cruise_buttons = self.cruise_buttons
prev_distance_button = self.distance_button
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
# Variables used for avoiding LKAS faults
self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
if self.loopback_lka_steering_cmd_updated:
self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"]
if self.CP.networkLocation == NetworkLocation.fwdCamera:
self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
# This is to avoid a fault where you engage while still moving backwards after shifting to D.
# An Equinox has been seen with an unsupported status (3), so only check if either wheel is in reverse (2)
left_whl_sign = -1 if pt_cp.vl["EBCMWheelSpdRear"]["RLWheelDir"] == 2 else 1
right_whl_sign = -1 if pt_cp.vl["EBCMWheelSpdRear"]["RRWheelDir"] == 2 else 1
self.parse_wheel_speeds(ret,
left_whl_sign * pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
right_whl_sign * pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
left_whl_sign * pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"],
right_whl_sign * pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"],
)
# sample rear wheel speeds to match the safety which only uses the rear CAN message
# standstill=True if ECM allows engagement with brake
ret.standstill = abs(pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"]) <= STANDSTILL_THRESHOLD and \
abs(pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"]) <= STANDSTILL_THRESHOLD
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
ret.gearShifter = self.parse_gear_shifter("T")
else:
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None))
ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"]
if self.CP.networkLocation == NetworkLocation.fwdCamera:
ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0
else:
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
# that the brake is being intermittently pressed without user interaction.
# To avoid a cruise fault we need to use a conservative brake position threshold
# https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
ret.brakePressed = ret.brake >= 8
# Regen braking is braking
if self.CP.transmissionType == TransmissionType.direct:
ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
ret.gasPressed = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. > 1e-5
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"]
ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"]
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
ret.steerFaultTemporary = self.lkas_status == 2
ret.steerFaultPermanent = self.lkas_status == 3
# 1 - open, 0 - closed
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
# 1 - latched
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1)
ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
if self.CP.networkLocation == NetworkLocation.fwdCamera:
if self.CP.carFingerprint not in ALT_ACCS and not self.CP_IQ.flags & GMFlagsIQ.NON_ACC:
ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
# This FCW signal only works for SDGM cars. CAM cars send FCW on GMLAN but this bit is always 0 for them
ret.stockFcw = cam_cp.vl["ASCMActiveCruiseControlStatus"]["FCWAlert"] != 0
if self.CP.pcmCruise:
# openpilot controls nonAdaptive when not pcmCruise
ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3)
else:
ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
if self.CP.carFingerprint not in SDGM_CAR:
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
if self.CP.enableBsm:
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
# Don't add event if transitioning from INIT, unless it's to an actual button
if self.cruise_buttons != CruiseButtons.UNPRESS or prev_cruise_buttons != CruiseButtons.INIT:
ret.buttonEvents = [
*create_button_events(self.cruise_buttons, prev_cruise_buttons, BUTTONS_DICT,
unpressed_btn=CruiseButtons.UNPRESS),
*create_button_events(self.distance_button, prev_distance_button,
{1: ButtonType.gapAdjustCruise})
]
if ret.vEgo < self.CP.minSteerSpeed:
ret.lowSpeedAlert = True
IQCarState.update(self, ret, can_parsers)
return ret, ret_iq
@staticmethod
def get_can_parsers(CP, CP_IQ):
pt_messages = []
if CP.networkLocation == NetworkLocation.fwdCamera:
pt_messages += [
("ASCMLKASteeringCmd", float('nan')),
]
loopback_messages = [
("ASCMLKASteeringCmd", float('nan')),
]
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
Bus.loopback: CANParser(DBC[CP.carFingerprint][Bus.pt], loopback_messages, 128),
}

84
artifacts/package_runtime/iqdbc/car/gm/fingerprints.py Normal file
View File

@@ -0,0 +1,84 @@
# ruff: noqa: E501
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
from iqdbc.car.gm.values import CAR
from iqdbc.lvbs.car.iq_fingerprints import extend_fingerprints
from iqdbc.lvbs.car.gm.iq_fingerprints import FINGERPRINTS_EXT
# Trailblazer also matches as a SILVERADO, TODO: split with fw versions
# FIXME: There are Equinox users with different message lengths, specifically 304 and 320
FINGERPRINTS = {
CAR.HOLDEN_ASTRA: [{
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 715: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7
}],
CAR.CHEVROLET_VOLT: [{
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 715: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
},
{
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 578: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1516: 8, 1601: 8, 1618: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2018: 8, 2020: 8, 2024: 8, 2028: 8
},
{
170: 8, 171: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 209: 7, 211: 2, 241: 6, 288: 5, 289: 1, 290: 1, 298: 2, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 368: 8, 381: 2, 384: 8, 386: 5, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 3, 508: 8, 512: 3, 528: 4, 530: 8, 532: 6, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 821: 4, 823: 7, 832: 8, 840: 5, 842: 5, 844: 8, 853: 8, 866: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7
}],
CAR.BUICK_LACROSSE: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 5, 707: 8, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 872: 1, 882: 8, 890: 1, 892: 2, 893: 1, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1916: 7, 1918: 7, 1919: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
}],
CAR.BUICK_REGAL: [{
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 8, 419: 8, 422: 4, 426: 8, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 463: 3, 479: 8, 481: 7, 485: 8, 487: 8, 489: 8, 495: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 884: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 8, 969: 8, 977: 8, 979: 8, 985: 8, 1001: 8, 1005: 6, 1009: 8, 1011: 8, 1013: 3, 1017: 8, 1020: 8, 1024: 8, 1025: 8, 1026: 8, 1027: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1032: 2, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 8, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 8, 1263: 8, 1265: 8, 1267: 8, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1603: 7, 1611: 8, 1618: 8, 1906: 8, 1907: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1930: 7, 2016: 8, 2018: 8, 2019: 8, 2024: 8, 2026: 8
}],
CAR.CADILLAC_ATS: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
}],
CAR.CHEVROLET_MALIBU: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8
}],
CAR.GMC_ACADIA: [{
190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7
},
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
}],
CAR.CADILLAC_ESCALADE: [{
170: 8, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 4, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
}],
CAR.CADILLAC_ESCALADE_ESV: [{
309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8
}],
CAR.CADILLAC_ESCALADE_ESV_2019: [{
715: 8, 840: 5, 717: 5, 869: 4, 880: 6, 289: 8, 454: 8, 842: 5, 460: 5, 463: 3, 801: 8, 170: 8, 190: 6, 241: 6, 201: 8, 417: 7, 211: 2, 419: 1, 398: 8, 426: 7, 487: 8, 442: 8, 451: 8, 452: 8, 453: 6, 479: 3, 311: 8, 500: 6, 647: 6, 193: 8, 707: 8, 197: 8, 209: 7, 199: 4, 455: 7, 313: 8, 481: 7, 485: 8, 489: 8, 249: 8, 393: 7, 407: 7, 413: 8, 422: 4, 431: 8, 501: 8, 499: 3, 810: 8, 508: 8, 381: 8, 462: 4, 532: 6, 562: 8, 386: 8, 761: 7, 573: 1, 554: 3, 719: 5, 560: 8, 1279: 4, 388: 8, 288: 5, 1005: 6, 497: 8, 844: 8, 961: 8, 967: 4, 977: 8, 979: 8, 985: 5, 1001: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 510: 8, 866: 4, 304: 1, 969: 8, 384: 4, 1033: 7, 1009: 8, 1034: 7, 1296: 4, 1930: 7, 1105: 5, 1013: 5, 1225: 7, 1919: 7, 320: 3, 534: 2, 352: 5, 298: 8, 1223: 2, 1233: 8, 608: 8, 1265: 8, 609: 6, 1267: 1, 1417: 8, 610: 6, 1906: 7, 611: 6, 612: 8, 613: 8, 208: 8, 564: 5, 309: 8, 1221: 5, 1280: 4, 1249: 8, 1907: 7, 1257: 6, 1300: 8, 1920: 7, 563: 5, 1322: 6, 1323: 4, 1328: 4, 1917: 7, 328: 1, 1912: 7, 1914: 7, 804: 3, 1918: 7
}],
CAR.CHEVROLET_BOLT_EUV: [{
189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
}],
CAR.CHEVROLET_SILVERADO: [{
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
},
{
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 3, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
}],
CAR.CHEVROLET_EQUINOX: [{
190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
},
{
190: 6, 201: 8, 211: 2, 717: 5, 241: 6, 451: 8, 298: 8, 452: 8, 453: 6, 479: 3, 485: 8, 249: 8, 500: 6, 587: 8, 1611: 8, 289: 8, 481: 7, 193: 8, 197: 8, 209: 7, 455: 7, 489: 8, 309: 8, 413: 8, 501: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 311: 8, 510: 8, 528: 5, 532: 6, 715: 8, 560: 8, 562: 8, 707: 8, 789: 5, 869: 4, 880: 6, 761: 7, 840: 5, 842: 5, 844: 8, 313: 8, 381: 8, 386: 8, 810: 8, 322: 7, 384: 4, 800: 6, 1033: 7, 1034: 7, 1296: 4, 753: 5, 388: 8, 288: 5, 497: 8, 463: 3, 304: 3, 977: 8, 1001: 8, 1280: 4, 320: 4, 352: 5, 563: 5, 565: 5, 1221: 5, 1011: 6, 1017: 8, 1020: 8, 1249: 8, 1300: 8, 328: 1, 1217: 8, 1233: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1930: 7, 1271: 8
}],
CAR.CADILLAC_XT4: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 806: 1, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1517: 8, 1601: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 8, 1924: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2024: 8, 2026: 8
}],
CAR.CHEVROLET_VOLT_2019: [{
170: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 573: 1, 577: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1268: 2, 1273: 3, 1275: 3, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1513: 8, 1516: 8, 1517: 8, 1601: 8, 1609: 8, 1611: 8, 1618: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1920: 8, 1922: 7, 1927: 7, 1930: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2004: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2023: 8, 2025: 8, 2028: 8, 2031: 8
}],
CAR.CHEVROLET_TRAVERSE: [{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 603: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 723: 4, 730: 4, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1346: 8, 1347: 8, 1355: 8, 1362: 8, 1417: 8, 1512: 8, 1514: 8, 1601: 8, 1602: 8, 1603: 7, 1609: 8, 1611: 8, 1613: 8, 1618: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 7, 1927: 8, 1930: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2004: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2024: 8, 2026: 8
}],
CAR.GMC_YUKON: [{
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 534: 2, 562: 8, 563: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 761: 7, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1355: 8, 1611: 8
}],
}
FW_VERSIONS: dict[str, dict[tuple, list[bytes]]] = {
}
FINGERPRINTS = extend_fingerprints(FINGERPRINTS, FINGERPRINTS_EXT)

171
artifacts/package_runtime/iqdbc/car/gm/gmcan.py Normal file
View File

@@ -0,0 +1,171 @@
from iqdbc.car.can_definitions import CanData
from iqdbc.car.gm.values import CAR
def create_buttons(packer, bus, idx, button):
values = {
"ACCButtons": button,
"RollingCounter": idx,
"ACCAlwaysOne": 1,
"DistanceButton": 0,
}
checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0)
checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0
checksum -= 2 * values["DistanceButton"]
values["SteeringButtonChecksum"] = checksum
return packer.make_can_msg("ASCMSteeringButton", bus, values)
def create_pscm_status(packer, bus, pscm_status):
values = {s: pscm_status[s] for s in [
"HandsOffSWDetectionMode",
"HandsOffSWlDetectionStatus",
"LKATorqueDeliveredStatus",
"LKADriverAppldTrq",
"LKATorqueDelivered",
"LKATotalTorqueDelivered",
"RollingCounter",
"PSCMStatusChecksum",
]}
checksum_mod = int(1 - values["HandsOffSWlDetectionStatus"]) << 5
values["HandsOffSWlDetectionStatus"] = 1
values["PSCMStatusChecksum"] += checksum_mod
return packer.make_can_msg("PSCMStatus", bus, values)
def create_steering_control(packer, bus, apply_torque, idx, lkas_active):
values = {
"LKASteeringCmdActive": lkas_active,
"LKASteeringCmd": apply_torque,
"RollingCounter": idx,
"LKASteeringCmdChecksum": 0x1000 - (lkas_active << 11) - (apply_torque & 0x7ff) - idx
}
return packer.make_can_msg("ASCMLKASteeringCmd", bus, values)
def create_adas_keepalive(bus):
dat = b"\x00\x00\x00\x00\x00\x00\x00"
return [CanData(0x409, dat, bus), CanData(0x40a, dat, bus)]
def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop):
values = {
"GasRegenCmdActive": enabled,
"RollingCounter": idx,
"GasRegenCmd": throttle,
"GasRegenFullStopActive": at_full_stop,
"GasRegenAccType": 1,
}
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[1]
values["GasRegenChecksum"] = ((1 - enabled) << 24) | \
(((0xff - dat[1]) & 0xff) << 16) | \
(((0xff - dat[2]) & 0xff) << 8) | \
((0x100 - dat[3] - idx) & 0xff)
return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_stop, at_full_stop, CP):
mode = 0x1
# TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake
if enabled and CP.carFingerprint in (CAR.CHEVROLET_BOLT_EUV,):
mode = 0x9
if apply_brake > 0:
mode = 0xa
if at_full_stop:
mode = 0xd
# TODO: this is to have GM bringing the car to complete stop,
# but currently it conflicts with OP controls, so turned off. Not set by all cars
#elif near_stop:
# mode = 0xb
brake = (0x1000 - apply_brake) & 0xfff
checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
values = {
"RollingCounter": idx,
"FrictionBrakeMode": mode,
"FrictionBrakeChecksum": checksum,
"FrictionBrakeCmd": -apply_brake
}
return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values)
def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_control, fcw):
target_speed = min(target_speed_kph, 255)
values = {
"ACCAlwaysOne": 1,
"ACCResumeButton": 0,
"ACCSpeedSetpoint": target_speed,
"ACCGapLevel": hud_control.leadDistanceBars * enabled, # 3 "far", 0 "inactive"
"ACCCmdActive": enabled,
"ACCAlwaysOne2": 1,
"ACCLeadCar": hud_control.leadVisible,
"FCWAlert": 0x3 if fcw else 0
}
return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values)
def create_adas_time_status(bus, tt, idx):
dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff,
((tt & 0xf) << 4) + (idx << 2)]
chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3]
chksum = chksum & 0xfff
dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12]
return CanData(0xa1, bytes(dat), bus)
def create_adas_steering_status(bus, idx):
dat = [idx << 6, 0xf0, 0x20, 0, 0, 0]
chksum = 0x60 + sum(dat)
dat += [chksum >> 8, chksum & 0xff]
return CanData(0x306, bytes(dat), bus)
def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
spd = int(speed_ms * 16) & 0xfff
accel = 0 & 0xfff
# 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L
#stick = 0x08
near_range_cutoff = 0x27
near_range_mode = 1 if spd <= near_range_cutoff else 0
far_range_mode = 1 - near_range_mode
dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0]
chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4]
dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff]
return CanData(0x308, bytes(dat), bus)
def create_adas_headlights_status(packer, bus):
values = {
"Always42": 0x42,
"Always4": 0x4,
}
return packer.make_can_msg("ASCMHeadlight", bus, values)
def create_lka_icon_command(bus, active, critical, steer):
if active and steer == 1:
if critical:
dat = b"\x50\xc0\x14"
else:
dat = b"\x50\x40\x18"
elif active:
if critical:
dat = b"\x40\xc0\x14"
else:
dat = b"\x40\x40\x18"
else:
dat = b"\x00\x00\x00"
return CanData(0x104c006c, dat, bus)

265
artifacts/package_runtime/iqdbc/car/gm/interface.py Normal file
View File

@@ -0,0 +1,265 @@
#!/usr/bin/env python3
from math import fabs, exp
import numpy as np
from iqdbc.car import get_safety_config, structs
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.gm.carcontroller import CarController
from iqdbc.car.gm.carstate import CarState
from iqdbc.car.gm.radar_interface import RadarInterface, RADAR_HEADER_MSG, CAMERA_DATA_HEADER_MSG
from iqdbc.car.gm.values import CAR, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, SDGM_CAR, ALT_ACCS, CanBus, GMSafetyFlags
from iqdbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType
from iqdbc.lvbs.car.gm.iq_interface import IQCarInterface
from iqdbc.lvbs.car.gm.iq_values import GMFlagsIQ, GMSafetyFlagsIQ
TransmissionType = structs.CarParams.TransmissionType
NetworkLocation = structs.CarParams.NetworkLocation
# Bolt Non-ACC uses the 4th (d) tune parameter; stock tunes zero it out.
NON_LINEAR_TORQUE_PARAMS_IQ = {
CAR.CHEVROLET_BOLT_NON_ACC: [2.24, 1.1, 0.28, -0.07],
CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN: [1.8, 1.1, 0.3, -0.045],
}
NON_LINEAR_TORQUE_PARAMS = {
CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122],
**NON_LINEAR_TORQUE_PARAMS_IQ,
}
class CarInterface(CarInterfaceBase, IQCarInterface):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
DRIVABLE_GEARS = (structs.CarState.GearShifter.sport, structs.CarState.GearShifter.low,
structs.CarState.GearShifter.eco, structs.CarState.GearShifter.manumatic)
def __init__(self, CP, CP_IQ):
CarInterfaceBase.__init__(self, CP, CP_IQ)
IQCarInterface.__init__(self, CP, CarInterfaceBase)
@staticmethod
def get_pid_accel_limits(CP, CP_IQ, current_speed, cruise_speed):
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
# Determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
@staticmethod
def get_steer_feedforward_volt(desired_angle, v_ego):
desired_angle *= 0.02904609
sigmoid = desired_angle / (1 + fabs(desired_angle))
return 0.10006696 * sigmoid * (v_ego + 3.12485927)
def get_steer_feedforward_function(self):
if self.CP.carFingerprint == CAR.CHEVROLET_VOLT:
return self.get_steer_feedforward_volt
else:
return CarInterfaceBase.get_steer_feedforward_default
def get_lataccel_torque_siglin(self) -> tuple[list[float], np.ndarray]:
def torque_from_lateral_accel_siglin_func(lateral_acceleration: float) -> float:
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
# This has big effect on the stability about 0 (noise when going straight)
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, d = non_linear_torque_params
d = d if NON_LINEAR_TORQUE_PARAMS_IQ.get(self.CP.carFingerprint) else 0.0
sig_input = a * lateral_acceleration
sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5)
steer_torque = (sig * b) + (lateral_acceleration * c) + d
return float(steer_torque)
lataccel_values = np.arange(-5.0, 5.0, 0.01)
torque_values = [torque_from_lateral_accel_siglin_func(x) for x in lataccel_values]
assert min(torque_values) < -1 and max(torque_values) > 1, "The torque values should cover the range [-1, 1]"
return torque_values, lataccel_values
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
torque_values, lataccel_values = self.get_lataccel_torque_siglin()
def torque_from_lateral_accel_siglin(lateral_acceleration: float, torque_params: structs.CarParams.LateralTorqueTuning):
return np.interp(lateral_acceleration, lataccel_values, torque_values)
return torque_from_lateral_accel_siglin
else:
return self.torque_from_lateral_accel_linear
def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType:
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
torque_values, lataccel_values = self.get_lataccel_torque_siglin()
def lateral_accel_from_torque_siglin(torque: float, torque_params: structs.CarParams.LateralTorqueTuning):
return np.interp(torque, torque_values, lataccel_values)
return lateral_accel_from_torque_siglin
else:
return self.lateral_accel_from_torque_linear
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "gm"
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.gm)]
ret.autoResumeSng = False
ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN]
if candidate in EV_CAR:
ret.transmissionType = TransmissionType.direct
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.EV.value
else:
ret.transmissionType = TransmissionType.automatic
ret.longitudinalTuning.kiBP = [5., 35.]
if candidate in (CAMERA_ACC_CAR | SDGM_CAR):
ret.alphaLongitudinalAvailable = candidate not in SDGM_CAR
ret.networkLocation = NetworkLocation.fwdCamera
ret.radarUnavailable = True # no radar
ret.pcmCruise = True
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM.value
ret.minEnableSpeed = -1 if candidate in SDGM_CAR else 5 * CV.KPH_TO_MS
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
# Tuning for experimental long
ret.longitudinalTuning.kiV = [2.0, 1.5]
if alpha_long:
ret.pcmCruise = False
ret.openpilotLongitudinalControl = True
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM_LONG.value
if candidate in ALT_ACCS:
ret.alphaLongitudinalAvailable = False
ret.openpilotLongitudinalControl = False
ret.minEnableSpeed = -1. # engage speed is decided by PCM
else: # ASCM, OBD-II harness
ret.openpilotLongitudinalControl = True
ret.networkLocation = NetworkLocation.gateway
# LRR messages can take up to a few seconds to start sending after ignition, check camera data as well which starts earlier
ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and CAMERA_DATA_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs
ret.pcmCruise = False # stock non-adaptive cruise control is kept off
# supports stop and go, but initial engage must (conservatively) be above 18mph
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
# Tuning
ret.longitudinalTuning.kiV = [2.4, 1.5]
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to iqdbc/car/tests/routes.py, we can remove it from this list.
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.CHEVROLET_MALIBU, CAR.BUICK_REGAL} or \
(ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable)
# Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below.
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
ret.steerLimitTimer = 0.4
ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking
if candidate == CAR.CHEVROLET_VOLT:
ret.lateralTuning.pid.kpBP = [0., 40.]
ret.lateralTuning.pid.kpV = [0., 0.17]
ret.lateralTuning.pid.kiBP = [0.]
ret.lateralTuning.pid.kiV = [0.]
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt()
ret.steerActuatorDelay = 0.2
elif candidate == CAR.GMC_ACADIA:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_LACROSSE:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CADILLAC_ESCALADE:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate in (CAR.CADILLAC_ESCALADE_ESV, CAR.CADILLAC_ESCALADE_ESV_2019):
ret.minEnableSpeed = -1. # engage speed is decided by pcm
if candidate == CAR.CADILLAC_ESCALADE_ESV:
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
ret.lateralTuning.pid.kf = 0.000045
else:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_BOLT_EUV:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_SILVERADO:
# On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
# with foot on brake to allow engagement, but this platform only has that check in the camera.
# TODO: check if this is split by EV/ICE with more platforms in the future
if ret.openpilotLongitudinalControl:
ret.minEnableSpeed = -1.
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_EQUINOX:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_TRAILBLAZER:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CADILLAC_XT4:
ret.steerActuatorDelay = 0.2
ret.minSteerSpeed = 30 * CV.MPH_TO_MS
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_VOLT_2019:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_TRAVERSE:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.GMC_YUKON:
ret.steerActuatorDelay = 0.5
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
ret.dashcamOnly = True # Needs steerRatio, tireStiffness, and lat accel factor tuning
return ret
@staticmethod
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]],
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
if candidate in (CAR.CHEVROLET_MALIBU_NON_ACC_9TH_GEN, CAR.CHEVROLET_BOLT_NON_ACC, CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN,
CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN):
stock_cp.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, stock_cp.lateralTuning)
elif candidate in (CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN, ):
CarInterfaceBase.configure_torque_tune(candidate, stock_cp.lateralTuning)
# Non-ACC cars steer/long via the forward camera and pcmCruise, not the ASCM.
if ret.flags & GMFlagsIQ.NON_ACC:
stock_cp.dashcamOnly = False
stock_cp.alphaLongitudinalAvailable = False
stock_cp.networkLocation = NetworkLocation.fwdCamera
stock_cp.openpilotLongitudinalControl = False
stock_cp.pcmCruise = True
stock_cp.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM.value
ret.iqSafetyFlags |= GMSafetyFlagsIQ.NON_ACC
stock_cp.minEnableSpeed = 24 * CV.MPH_TO_MS
stock_cp.minSteerSpeed = 3.0
# Untested Non-ACC platforms ship dashcam-only pending user validation.
if candidate in (CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN,
CAR.CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN, CAR.CADILLAC_CT6_NON_ACC_1ST_GEN,
CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN, CAR.CADILLAC_XT5_NON_ACC_1ST_GEN):
stock_cp.dashcamOnly = True
return ret

99
artifacts/package_runtime/iqdbc/car/gm/radar_interface.py Normal file
View File

@@ -0,0 +1,99 @@
#!/usr/bin/env python3
import math
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.gm.values import DBC, CanBus
from iqdbc.car.interfaces import RadarInterfaceBase
RADAR_HEADER_MSG = 1120 # F_LRR_Obj_Header
CAMERA_DATA_HEADER_MSG = 1056 # F_Vision_Obj_Header
SLOT_1_MSG = RADAR_HEADER_MSG + 1
NUM_SLOTS = 20
# Actually it's 0x47f, but can parser only reports
# messages that are present in DBC
LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
def create_radar_can_parser(car_fingerprint):
# C1A-ARS3-A by Continental
radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS))
signals = list(zip(['FLRRNumValidTargets',
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] +
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
[RADAR_HEADER_MSG] * 7 + radar_targets * 6, strict=True))
messages = list({(s[1], 14) for s in signals})
return CANParser(DBC[car_fingerprint][Bus.radar], messages, CanBus.OBSTACLE)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP, CP_IQ):
super().__init__(CP, CP_IQ)
self.rcp = None if CP.radarUnavailable else create_radar_can_parser(CP.carFingerprint)
self.trigger_msg = LAST_RADAR_MSG
self.updated_messages = set()
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
vls = self.rcp.update(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
ret = structs.RadarData()
header = self.rcp.vl[RADAR_HEADER_MSG]
fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \
header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \
header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt']
if not self.rcp.can_valid:
ret.errors.canError = True
if fault:
ret.errors.radarFault = True
currentTargets = set()
num_targets = header['FLRRNumValidTargets']
# Not all radar messages describe targets,
# no need to monitor all of the self.rcp.msgs_upd
for ii in self.updated_messages:
if ii == RADAR_HEADER_MSG:
continue
if num_targets == 0:
break
cpt = self.rcp.vl[ii]
# Zero distance means it's an empty target slot
if cpt['TrkRange'] > 0.0:
targetId = cpt['TrkObjectID']
currentTargets.add(targetId)
if targetId not in self.pts:
self.pts[targetId] = structs.RadarData.RadarPoint()
self.pts[targetId].trackId = targetId
distance = cpt['TrkRange']
self.pts[targetId].dRel = distance # from front of car
# From driver's pov, left is positive
self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance
self.pts[targetId].vRel = cpt['TrkRangeRate']
self.pts[targetId].aRel = float('nan')
self.pts[targetId].yvRel = float('nan')
for oldTarget in list(self.pts.keys()):
if oldTarget not in currentTargets:
del self.pts[oldTarget]
ret.points = list(self.pts.values())
self.updated_messages.clear()
return ret

0
artifacts/package_runtime/iqdbc/car/gm/tests/__init__.py Normal file
View File

20
artifacts/package_runtime/iqdbc/car/gm/tests/test_gm.py Normal file
View File

@@ -0,0 +1,20 @@
from parameterized import parameterized
from iqdbc.car.gm.fingerprints import FINGERPRINTS
from iqdbc.car.gm.values import CAMERA_ACC_CAR, GM_RX_OFFSET
CAMERA_DIAGNOSTIC_ADDRESS = 0x24b
class TestGMFingerprint:
@parameterized.expand(FINGERPRINTS.items())
def test_can_fingerprints(self, car_model, fingerprints):
assert len(fingerprints) > 0
assert all(len(finger) for finger in fingerprints)
# The camera can sometimes be communicating on startup
if car_model in CAMERA_ACC_CAR:
for finger in fingerprints:
for required_addr in (CAMERA_DIAGNOSTIC_ADDRESS, CAMERA_DIAGNOSTIC_ADDRESS + GM_RX_OFFSET):
assert finger.get(required_addr) == 8, required_addr

343
artifacts/package_runtime/iqdbc/car/gm/values.py Normal file
View File

@@ -0,0 +1,343 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag
from iqdbc.car import Bus, PlatformConfig, DbcDict, Platforms, CarSpecs
from iqdbc.car.structs import CarParams
from iqdbc.car.docs_definitions import CarDocs, CarFootnote, CarHarness, CarParts, Column, SupportType
from iqdbc.lvbs.car.gm.iq_values import GMFlagsIQ
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = CarParams.Ecu
class CarControllerParams:
STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output
STEER_STEP = 3 # Active control frames per command (~33hz)
INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz)
STEER_DELTA_UP = 10 # Delta rates require review due to observed EPS weakness
STEER_DELTA_DOWN = 15
STEER_DRIVER_ALLOWANCE = 65
STEER_DRIVER_MULTIPLIER = 4
STEER_DRIVER_FACTOR = 100
NEAR_STOP_BRAKE_PHASE = 0.5 # m/s
# Heartbeat for dash "Service Adaptive Cruise" and "Service Front Camera"
ADAS_KEEPALIVE_STEP = 100
CAMERA_KEEPALIVE_STEP = 100
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
# perform the closed loop control, and might need some
# to apply some more braking if we're on a downhill slope.
# Our controller should still keep the 2 second average above
# -3.5 m/s^2 as per planner limits
ACCEL_MAX = 2. # m/s^2
ACCEL_MIN = -4. # m/s^2
def __init__(self, CP):
# Gas/brake lookups
self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
if CP.carFingerprint in (CAMERA_ACC_CAR | SDGM_CAR):
self.MAX_GAS = 1346.0
self.MAX_ACC_REGEN = -540.0
self.INACTIVE_REGEN = -500.0
# Camera ACC vehicles have no regen while enabled.
# Camera transitions to MAX_ACC_REGEN from zero gas and uses friction brakes instantly
max_regen_acceleration = 0.
else:
self.MAX_GAS = 1018.0 # Safety limit, not ACC max. Stock ACC >2042 from standstill.
self.MAX_ACC_REGEN = -650.0 # Max ACC regen is slightly less than max paddle regen
self.INACTIVE_REGEN = -650.0
# ICE has much less engine braking force compared to regen in EVs,
# lower threshold removes some braking deadzone
max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1
self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX]
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, 0., self.MAX_GAS]
self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration]
self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
class GMSafetyFlags(IntFlag):
HW_CAM = 1
HW_CAM_LONG = 2
EV = 4
class Footnote(Enum):
SETUP = CarFootnote(
"See more setup details for <a href=\"https://github.com/commaai/openpilot/wiki/gm\" target=\"_blank\">GM</a>.",
Column.MAKE, setup_note=True)
@dataclass
class GMCarDocs(CarDocs):
package: str = "Adaptive Cruise Control (ACC)"
def init_make(self, CP: CarParams):
if CP.networkLocation == CarParams.NetworkLocation.fwdCamera:
if CP.carFingerprint in SDGM_CAR:
self.car_parts = CarParts.common([CarHarness.gmsdgm])
else:
self.car_parts = CarParts.common([CarHarness.gm])
else:
self.footnotes.insert(0, Footnote.SETUP)
self.car_parts = CarParts.common([CarHarness.obd_ii])
@dataclass(frozen=True, kw_only=True)
class GMCarSpecs(CarSpecs):
tireStiffnessFactor: float = 0.444 # not optimized yet
@dataclass
class GMPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {
Bus.pt: 'gm_global_a_powertrain_generated',
Bus.radar: 'gm_global_a_object',
Bus.chassis: 'gm_global_a_chassis',
})
@dataclass
class GMASCMPlatformConfig(GMPlatformConfig):
def init(self):
# ASCM is supported, but due to a janky install and hardware configuration, we are not showing in the car docs
self.car_docs = []
@dataclass
class GMSDGMPlatformConfig(GMPlatformConfig):
def init(self):
# Don't show in docs until the harness is sold. See https://github.com/commaai/openpilot/issues/32471
self.car_docs = []
@dataclass
class GMNonAccCarDocs(GMCarDocs):
package: str = "No Adaptive Cruise Control (Non-ACC)"
support_type: SupportType = SupportType.COMMUNITY
support_link: str = "community"
@dataclass
class GMNonSccPlatformConfig(GMPlatformConfig):
def init(self):
self.iq_flags |= GMFlagsIQ.NON_ACC
class CAR(Platforms):
HOLDEN_ASTRA = GMASCMPlatformConfig(
[GMCarDocs("Holden Astra 2017")],
GMCarSpecs(mass=1363, wheelbase=2.662, steerRatio=15.7, centerToFrontRatio=0.4),
)
CHEVROLET_VOLT = GMASCMPlatformConfig(
[GMCarDocs("Chevrolet Volt 2017-18", min_enable_speed=0, video="https://youtu.be/QeMCN_4TFfQ")],
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=0.469),
)
CADILLAC_ATS = GMASCMPlatformConfig(
[GMCarDocs("Cadillac ATS Premium Performance 2018")],
GMCarSpecs(mass=1601, wheelbase=2.78, steerRatio=15.3),
)
CHEVROLET_MALIBU = GMASCMPlatformConfig(
[GMCarDocs("Chevrolet Malibu Premier 2017")],
GMCarSpecs(mass=1496, wheelbase=2.83, steerRatio=15.8, centerToFrontRatio=0.4),
)
GMC_ACADIA = GMASCMPlatformConfig(
[GMCarDocs("GMC Acadia 2018", video="https://www.youtube.com/watch?v=0ZN6DdsBUZo")],
GMCarSpecs(mass=1975, wheelbase=2.86, steerRatio=14.4, centerToFrontRatio=0.4),
)
BUICK_LACROSSE = GMASCMPlatformConfig(
[GMCarDocs("Buick LaCrosse 2017-19", "Driver Confidence Package 2")],
GMCarSpecs(mass=1712, wheelbase=2.91, steerRatio=15.8, centerToFrontRatio=0.4),
)
BUICK_REGAL = GMASCMPlatformConfig(
[GMCarDocs("Buick Regal Essence 2018")],
GMCarSpecs(mass=1714, wheelbase=2.83, steerRatio=14.4, centerToFrontRatio=0.4),
)
CADILLAC_ESCALADE = GMASCMPlatformConfig(
[GMCarDocs("Cadillac Escalade 2017", "Driver Assist Package")],
GMCarSpecs(mass=2564, wheelbase=2.95, steerRatio=17.3),
)
CADILLAC_ESCALADE_ESV = GMASCMPlatformConfig(
[GMCarDocs("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS")],
GMCarSpecs(mass=2739, wheelbase=3.302, steerRatio=17.3, tireStiffnessFactor=1.0),
)
CADILLAC_ESCALADE_ESV_2019 = GMASCMPlatformConfig(
[GMCarDocs("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS")],
CADILLAC_ESCALADE_ESV.specs,
)
CHEVROLET_BOLT_EUV = GMPlatformConfig(
[
GMCarDocs("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim, without Super Cruise Package", video="https://youtu.be/xvwzGMUA210"),
GMCarDocs("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"),
],
GMCarSpecs(mass=1669, wheelbase=2.63779, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0),
)
CHEVROLET_SILVERADO = GMPlatformConfig(
[
GMCarDocs("Chevrolet Silverado 1500 2020-21", "Safety Package II"),
GMCarDocs("GMC Sierra 1500 2020-21", "Driver Alert Package II", video="https://youtu.be/5HbNoBLzRwE"),
],
GMCarSpecs(mass=2450, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0),
)
CHEVROLET_EQUINOX = GMPlatformConfig(
[GMCarDocs("Chevrolet Equinox 2019-22")],
GMCarSpecs(mass=1588, wheelbase=2.72, steerRatio=14.4, centerToFrontRatio=0.4),
)
CHEVROLET_TRAILBLAZER = GMPlatformConfig(
[GMCarDocs("Chevrolet Trailblazer 2021-22")],
GMCarSpecs(mass=1345, wheelbase=2.64, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0),
)
CADILLAC_XT4 = GMSDGMPlatformConfig(
[GMCarDocs("Cadillac XT4 2023", "Driver Assist Package")],
GMCarSpecs(mass=1660, wheelbase=2.78, steerRatio=14.4, centerToFrontRatio=0.4),
)
CHEVROLET_VOLT_2019 = GMSDGMPlatformConfig(
[GMCarDocs("Chevrolet Volt 2019", "Adaptive Cruise Control (ACC) & LKAS")],
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=15.7, centerToFrontRatio=0.45),
)
CHEVROLET_TRAVERSE = GMSDGMPlatformConfig(
[GMCarDocs("Chevrolet Traverse 2022-23", "RS, Premier, or High Country Trim")],
GMCarSpecs(mass=1955, wheelbase=3.07, steerRatio=17.9, centerToFrontRatio=0.4),
)
GMC_YUKON = GMPlatformConfig(
[GMCarDocs("GMC Yukon 2019-20", "Adaptive Cruise Control (ACC) & LKAS")],
GMCarSpecs(mass=2490, wheelbase=2.94, steerRatio=17.3, centerToFrontRatio=0.5, tireStiffnessFactor=1.0),
)
# IQ.Pilot Non-ACC camera-harness ports (no factory adaptive cruise).
CHEVROLET_BOLT_NON_ACC = GMNonSccPlatformConfig(
[GMNonAccCarDocs("Chevrolet Bolt EV Non-ACC 2017")],
CHEVROLET_BOLT_EUV.specs,
)
CHEVROLET_BOLT_NON_ACC_1ST_GEN = GMNonSccPlatformConfig(
[GMNonAccCarDocs("Chevrolet Bolt EV Non-ACC 2018-21")],
CHEVROLET_BOLT_EUV.specs,
)
CHEVROLET_BOLT_NON_ACC_2ND_GEN = GMNonSccPlatformConfig(
[
GMNonAccCarDocs("Chevrolet Bolt EUV LT Non-ACC 2022-23"),
GMNonAccCarDocs("Chevrolet Bolt EV LT Non-ACC 2022-23"),
],
CHEVROLET_BOLT_EUV.specs,
)
CHEVROLET_EQUINOX_NON_ACC_3RD_GEN = GMNonSccPlatformConfig(
[GMNonAccCarDocs("Chevrolet Equinox Non-ACC 2019-22")],
CHEVROLET_EQUINOX.specs,
)
CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN = GMNonSccPlatformConfig(
[GMNonAccCarDocs("Chevrolet Suburban Non-ACC 2016-20")],
CarSpecs(mass=2731, wheelbase=3.302, steerRatio=17.3, centerToFrontRatio=0.49),
)
CADILLAC_CT6_NON_ACC_1ST_GEN = GMNonSccPlatformConfig(
[GMNonAccCarDocs("Cadillac CT6 Non-ACC 2017-18")],
CarSpecs(mass=2358, wheelbase=3.11, steerRatio=17.7, centerToFrontRatio=0.4),
)
CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN = GMNonSccPlatformConfig(
[GMNonAccCarDocs("Chevrolet Trailblazer Non-ACC 2021-22")],
CHEVROLET_TRAILBLAZER.specs,
)
CHEVROLET_MALIBU_NON_ACC_9TH_GEN = GMNonSccPlatformConfig(
[GMNonAccCarDocs("Chevrolet Malibu Non-ACC 2016-23")],
CarSpecs(mass=1450, wheelbase=2.8, steerRatio=15.8, centerToFrontRatio=0.4),
)
CADILLAC_XT5_NON_ACC_1ST_GEN = GMNonSccPlatformConfig(
[GMNonAccCarDocs("Cadillac XT5 Non-ACC 2018")],
CarSpecs(mass=1810, wheelbase=2.86, steerRatio=16.34, centerToFrontRatio=0.5),
)
class CruiseButtons:
INIT = 0
UNPRESS = 1
RES_ACCEL = 2
DECEL_SET = 3
MAIN = 5
CANCEL = 6
class AccState:
OFF = 0
ACTIVE = 1
FAULTED = 3
STANDSTILL = 4
class CanBus:
POWERTRAIN = 0
OBSTACLE = 1
CAMERA = 2
CHASSIS = 2
LOOPBACK = 128
DROPPED = 192
# In a Data Module, an identifier is a string used to recognize an object,
# either by itself or together with the identifiers of parent objects.
# Each returns a 4 byte hex representation of the decimal part number. `b"\x02\x8c\xf0'"` -> 42790951
GM_BOOT_SOFTWARE_PART_NUMER_REQUEST = b'\x1a\xc0' # likely does not contain anything useful
GM_SOFTWARE_MODULE_1_REQUEST = b'\x1a\xc1'
GM_SOFTWARE_MODULE_2_REQUEST = b'\x1a\xc2'
GM_SOFTWARE_MODULE_3_REQUEST = b'\x1a\xc3'
# Part number of XML data file that is used to configure ECU
GM_XML_DATA_FILE_PART_NUMBER = b'\x1a\x9c'
GM_XML_CONFIG_COMPAT_ID = b'\x1a\x9b' # used to know if XML file is compatible with the ECU software/hardware
# This DID is for identifying the part number that reflects the mix of hardware,
# software, and calibrations in the ECU when it first arrives at the vehicle assembly plant.
# If there's an Alpha Code, it's associated with this part number and stored in the DID $DB.
GM_END_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcb'
GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdb'
GM_BASE_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcc'
GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdc'
GM_FW_RESPONSE = b'\x5a'
GM_FW_REQUESTS = [
GM_BOOT_SOFTWARE_PART_NUMER_REQUEST,
GM_SOFTWARE_MODULE_1_REQUEST,
GM_SOFTWARE_MODULE_2_REQUEST,
GM_SOFTWARE_MODULE_3_REQUEST,
GM_XML_DATA_FILE_PART_NUMBER,
GM_XML_CONFIG_COMPAT_ID,
GM_END_MODEL_PART_NUMBER_REQUEST,
GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST,
GM_BASE_MODEL_PART_NUMBER_REQUEST,
GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST,
]
GM_RX_OFFSET = 0x400
FW_QUERY_CONFIG = FwQueryConfig(
requests=[request for req in GM_FW_REQUESTS for request in [
Request(
[StdQueries.SHORT_TESTER_PRESENT_REQUEST, req],
[StdQueries.SHORT_TESTER_PRESENT_RESPONSE, GM_FW_RESPONSE + bytes([req[-1]])],
rx_offset=GM_RX_OFFSET,
bus=0,
logging=True,
),
]],
extra_ecus=[(Ecu.fwdCamera, 0x24b, None)],
)
# TODO: detect most of these sets live
EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_2019, CAR.CHEVROLET_BOLT_EUV,
# Non-ACC EV ports
CAR.CHEVROLET_BOLT_NON_ACC, CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN, CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN}
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
CAMERA_ACC_CAR = {CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_EQUINOX, CAR.CHEVROLET_TRAILBLAZER, CAR.GMC_YUKON}
# Alt ASCMActiveCruiseControlStatus
ALT_ACCS = {CAR.GMC_YUKON}
# We're integrated at the Safety Data Gateway Module on these cars
SDGM_CAR = {CAR.CADILLAC_XT4, CAR.CHEVROLET_VOLT_2019, CAR.CHEVROLET_TRAVERSE}
STEER_THRESHOLD = 1.0
DBC = CAR.create_dbc_map()

0
artifacts/package_runtime/iqdbc/car/honda/__init__.py Normal file
View File

490
artifacts/package_runtime/iqdbc/car/honda/carcontroller.py Normal file
View File

@@ -0,0 +1,490 @@
import numpy as np
import math
from iqdbc.can import CANPacker
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, rate_limit, make_tester_present_msg, structs
from iqdbc.car.common.pid import PIDController
from iqdbc.car.honda import dash_lane, dash_objects, hondacan
from iqdbc.car.honda.values import CAR, CruiseButtons, CruiseSettings, HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS, \
HONDA_BOSCH_TJA_CONTROL, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
from iqdbc.car.interfaces import CarControllerBase
from iqdbc.lvbs.car.honda.aol import AolCarController
from iqdbc.lvbs.car.honda.gas_interceptor import GasInterceptorCarController
VisualAlert = structs.CarControl.HUDControl.VisualAlert
LongCtrlState = structs.CarControl.Actuators.LongControlState
def compute_gb_honda_bosch(accel, speed):
# TODO returns 0s, is unused
return 0.0, 0.0
def compute_gb_honda_nidec(accel, speed):
creep_brake = 0.0
creep_speed = 2.3
creep_brake_value = 0.15
if speed < creep_speed:
creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
gb = float(accel) / 4.8 - creep_brake
return np.clip(gb, 0.0, 1.0), np.clip(-gb, 0.0, 1.0)
def compute_gas_brake(accel, speed, fingerprint):
if fingerprint in HONDA_BOSCH:
return compute_gb_honda_bosch(accel, speed)
else:
return compute_gb_honda_nidec(accel, speed)
# TODO not clear this does anything useful
def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
brake_hyst_on = 0.02 # to activate brakes exceed this value
brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
# *** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
brake = 0.
braking = brake > 0.
# for small brake oscillations within brake_hyst_gap, don't change the brake command
if brake == 0.:
brake_steady = 0.
elif brake > brake_steady + brake_hyst_gap:
brake_steady = brake - brake_hyst_gap
elif brake < brake_steady - brake_hyst_gap:
brake_steady = brake + brake_hyst_gap
brake = brake_steady
return brake, braking, brake_steady
def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts, ts):
pump_on = False
# reset pump timer if:
# - there is an increment in brake request
# - we are applying steady state brakes and we haven't been running the pump
# for more than 20s (to prevent pressure bleeding)
if apply_brake > apply_brake_last or (ts - last_pump_ts > 20. and apply_brake > 0):
last_pump_ts = ts
# once the pump is on, run it for at least 0.2s
if ts - last_pump_ts < 0.2 and apply_brake > 0:
pump_on = True
return pump_on, last_pump_ts
def process_hud_alert(hud_alert):
alert_fcw = False
alert_steer_required = False
# Make sure FCW is prioritized over steering required
# TODO: implement separate available LDW alert
if hud_alert == VisualAlert.fcw:
alert_fcw = True
elif hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw):
alert_steer_required = True
return alert_fcw, alert_steer_required
class CarController(CarControllerBase, AolCarController, GasInterceptorCarController):
def __init__(self, dbc_names, CP, CP_IQ):
CarControllerBase.__init__(self, dbc_names, CP, CP_IQ)
AolCarController.__init__(self)
GasInterceptorCarController.__init__(self, CP, CP_IQ)
self.packer = CANPacker(dbc_names[Bus.pt])
self.params = CarControllerParams(CP)
self.CAN = hondacan.CanBus(CP)
self.tja_control = CP.carFingerprint in HONDA_BOSCH_TJA_CONTROL
self.lane_renderer = dash_lane.LanePathRenderer()
self.dash_object_author = dash_objects.DashObjectAuthor()
self.rendered_lane = dash_lane.RenderedLane()
self.lkas_hud_key = None
self.lkas_state_change_frames = 0
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
self.apply_brake_last = 0
self.last_pump_ts = 0.
self.stopping_counter = 0
self.accel = 0.0
self.speed = 0.0
self.gas = 0.0
self.brake = 0.0
self.last_torque = 0.0
self.bosch_last_gas = 0
self.lkas_button_send_remaining = 0
self.last_lkas_button_frame = 0
self.radar_disable_counter = 0
self.radar_mux = 0
# stock RADAR_HUD_CANFD raises its CMBS bit only for a short burst after ACC engages; 10Hz hud ticks
self.radar_hud_pulse = 0
self.last_acc_enabled = False
self.gasfactor = 1.0
self.gasfactor_before_maxgas = 1.0
self.windfactor = 1.0
self.windfactor_before_maxgas = 1.0
self.windfactor_before_brake = 0.0
self.pitch = 0.0
self.brake_pid = PIDController(k_p=0.0, k_i=1.0, pos_limit=0.0, neg_limit=-2.0, rate=50)
self.brake_pid.reset()
def update(self, CC, CC_IQ, CS, now_nanos):
AolCarController.update(self, self.CP, CC, CC_IQ)
gas_pedal_force = 0.0
min_gas = self.params.BOSCH_GAS_LOOKUP_BP[0]
actuators = CC.actuators
hud_control = CC.hudControl
hud_v_cruise = hud_control.setSpeed / CS.v_cruise_factor if hud_control.speedVisible else 255
pcm_cancel_cmd = CC.cruiseControl.cancel
if len(CC.orientationNED) == 3:
self.pitch = CC.orientationNED[1]
hill_brake = math.sin(self.pitch) * ACCELERATION_DUE_TO_GRAVITY
if CC.longActive:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel + hill_brake, CS.out.vEgo, self.CP.carFingerprint)
else:
accel = 0.0
gas, brake = 0.0, 0.0
# *** rate limit steer ***
limited_torque = rate_limit(actuators.torque, self.last_torque, -self.params.STEER_DELTA_DOWN * DT_CTRL,
self.params.STEER_DELTA_UP * DT_CTRL)
self.last_torque = limited_torque
# *** apply brake hysteresis ***
pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
CS.out.vEgo, self.CP.carFingerprint)
# *** rate limit after the enable check ***
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., 3 * DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
alert_fcw, alert_steer_required = process_hud_alert(hud_control.visualAlert)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_torque = int(np.interp(-limited_torque * self.params.STEER_MAX,
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
# Send CAN commands
can_sends = []
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and self.CP.openpilotLongitudinalControl:
if self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.stock_acc_alive:
# CAN FD: the radar is silenced from here rather than from CarInterface.init(), and only once
# the comma relay is confirmed open: init() ran under the ELM327 safety mode, so the
# replacement ACC_CONTROL stream was blocked until the safety-mode switch landed, and whenever
# that took longer than ~110ms after radar silence the brake module latched CRUISE_FAULT for
# the whole drive. With the relay open the replacement stream starts within a few frames of
# radar silence (see CS.stock_acc_alive), well inside the fault threshold
if CS.canfd_relay_open:
if self.radar_disable_counter % 50 == 0:
# UDS extended diagnostic session, required before CommunicationControl
can_sends.append((0x18DAB0F1, b'\x02\x10\x03\x00\x00\x00\x00\x00', self.CAN.pt))
elif self.radar_disable_counter % 50 == 5:
# UDS CommunicationControl disableRxAndTx (0x80 suppresses the response), retried every
# 0.5s until the radar goes silent
can_sends.append((0x18DAB0F1, b'\x03\x28\x83\x03\x00\x00\x00\x00', self.CAN.pt))
self.radar_disable_counter += 1
elif self.frame % 10 == 0:
# tester present - w/ no response (keeps radar disabled)
can_sends.append(make_tester_present_msg(0x18DAB0F1, self.CAN.pt, suppress_response=True))
# simulate the disabled canfd radar to prevent faults. These look-alikes are consumed by both the
# camera (behind the relay, on the camera bus) and the powertrain: openpilot's own TX is not
# forwarded across the open relay, so each frame is packed exactly once (the packer's
# counter/checksum only advance once per cycle) and the identical bytes are mirrored onto both
# buses (re-packing would double-increment the counter and desync the buses). While the stock
# radar is still transmitting it authors all of these itself
if self.CP.carFingerprint in HONDA_BOSCH_CANFD and self.CP.openpilotLongitudinalControl and not CS.stock_acc_alive:
if CC.enabled and not self.last_acc_enabled:
self.radar_hud_pulse = 30 # ~3s at 10Hz, matching the stock 2-6s engage burst
self.last_acc_enabled = CC.enabled
radar_msgs = []
if CS.hud_tick:
radar_msgs.append(hondacan.create_radar_hud_canfd(self.packer, self.CAN.pt, CC.enabled, self.radar_hud_pulse > 0))
if self.radar_hud_pulse > 0:
self.radar_hud_pulse -= 1
if CS.supp_tick:
radar_msgs.append(hondacan.create_canfd_supplemental(self.packer, self.CAN.pt))
if CS.radar_50hz_tick:
# Cycle the radar MUX through the stock banks: 1-10, 17-26, 33-42, 49-58. This counter also
# drives the LANE_PATH/HUD_OBJECTS mux below: it advances exactly one step per transmitted
# frame, so the sweep stays contiguous even when a tick is missed (a frame-derived mux left
# holes in the sweep the stock radar never produces).
# These must be elif: a bare `if` at a bank start would fall through to the increment,
# skipping the bank-start values (17, 33, 49)
if self.radar_mux >= 58:
self.radar_mux = 1
elif self.radar_mux == 10:
self.radar_mux = 17
elif self.radar_mux == 26:
self.radar_mux = 33
elif self.radar_mux == 42:
self.radar_mux = 49
else:
self.radar_mux += 1
if CS.radar_5hz_tick:
# RADAR_LEAD's LANE_PATH_LENGTH must track the valid-point count of the LANE_PATH sweep being
# authored, and LEFT_LANE/RIGHT_LANE the per-side line-detected status, in lockstep with the
# stock radar's behavior or the dash won't draw the lane lines
radar_msgs.extend(hondacan.create_canfd_5hz_radar_messages(self.packer, self.CAN.pt, CS.radar_ref_counter,
dash_lane.canfd_lane_length(self.rendered_lane),
dash_lane.LANE_LINE_ON if self.rendered_lane.left_line else 0,
dash_lane.LANE_LINE_ON if self.rendered_lane.right_line else 0))
for addr, dat, _ in radar_msgs:
can_sends.append((addr, dat, self.CAN.pt))
can_sends.append((addr, dat, self.CAN.camera))
# Send steering command.
can_sends.append(hondacan.create_steering_control(self.packer, self.CAN, apply_torque, CC.latActive, self.tja_control))
# wind brake from air resistance decel at high speed
wind_brake = np.interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) * self.windfactor # not in m/s2 units
wind_brake_ms2 = np.interp(CS.out.vEgo, [0.0, 13.4, 22.4, 31.3, 40.2], [0.000, 0.049, 0.136, 0.267, 0.441]) # in m/s2 units
# all of this is only relevant for HONDA NIDEC
max_accel = np.interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V)
# TODO this 1.44 is just to maintain previous behavior
pcm_speed_BP = [-wind_brake,
-wind_brake * (3 / 4),
0.0,
0.5]
# The Honda ODYSSEY seems to have different PCM_ACCEL
# msgs, is it other cars too?
if self.CP_IQ.enableGasInterceptor or not CC.longActive:
pcm_speed = 0.0
pcm_accel = int(0.0)
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
pcm_speed_V = [0.0,
np.clip(CS.out.vEgo - 3.0, 0.0, 100.0),
np.clip(CS.out.vEgo + 0.0, 0.0, 100.0),
np.clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
pcm_speed = float(np.interp(gas - brake, pcm_speed_BP, pcm_speed_V))
pcm_accel = int(1.0 * self.params.NIDEC_GAS_MAX)
else:
pcm_speed_V = [0.0,
np.clip(CS.out.vEgo - 2.0, 0.0, 100.0),
np.clip(CS.out.vEgo + 2.0, 0.0, 100.0),
np.clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
pcm_speed = float(np.interp(gas - brake, pcm_speed_BP, pcm_speed_V))
pcm_accel = int(np.clip((accel / 1.44) / max_accel, 0.0, 1.0) * self.params.NIDEC_GAS_MAX)
if not self.CP.openpilotLongitudinalControl:
if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD:
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CAN))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.CANCEL, 0, CS.scm_ambient_light,
self.CP.carFingerprint))
elif CC.cruiseControl.resume:
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.RES_ACCEL, 0, CS.scm_ambient_light,
self.CP.carFingerprint))
else:
# Send gas and brake commands.
if self.frame % 2 == 0:
ts = self.frame * DT_CTRL
if self.CP.carFingerprint in HONDA_BOSCH:
# low-speed extra brake: the fixed accel command under-delivers approaching a stop, so an
# integral-only term closes the gap, releasing at 1 m/s^3 once out of the window
if (accel < min_gas) and (CS.out.vEgo < 3.0) and not (-1e-3 < CS.out.vEgo < 1e-3):
brake_addon = self.brake_pid.update(error=accel - CS.out.aEgo, speed=CS.out.vEgo)
target_accel = min(accel, accel + brake_addon)
else:
if (self.brake_pid.i < 0.0) and (accel < min_gas):
self.brake_pid.i = min(0.0, self.brake_pid.i + 0.02)
else:
self.brake_pid.reset()
target_accel = min(accel, accel + self.brake_pid.i)
self.accel = float(np.clip(target_accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX))
# not using self.accel since the brake pid resets with the gas pedal
gas_pedal_force = accel + wind_brake_ms2 * self.windfactor + hill_brake
# Live-learn gas pedal adjustments when openpilot is controlling gas.
if (actuators.longControlState == LongCtrlState.pid) and (not CS.out.gasPressed):
gas_error = accel - CS.out.aEgo
if gas_error != 0.0 and gas_pedal_force > min_gas:
if self.CP.carFingerprint in (CAR.HONDA_INSIGHT, CAR.HONDA_CIVIC_BOSCH): # gas pedal reacts too slowly
learn_speed = 150
elif self.CP.carFingerprint == CAR.ACURA_RDX_3G: # prevent overreacting to turbo lag
learn_speed = 300
else:
learn_speed = 50
self.gasfactor = np.clip(self.gasfactor + gas_error / learn_speed * (gas_pedal_force - min_gas), 0.01, 3.0)
if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0):
wind_learn_speed = 100 if self.CP.carFingerprint == CAR.ACURA_RDX_3G else 1000
wind_adjust = 1 + wind_brake_ms2 / wind_learn_speed
self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0 / wind_adjust), 0.1, 3.0)
if gas_pedal_force <= min_gas:
self.windfactor = max(self.windfactor, self.windfactor_before_brake)
else:
self.windfactor_before_brake = self.windfactor
if gas_pedal_force >= self.params.BOSCH_ACCEL_MAX:
self.gasfactor = min(self.gasfactor, self.gasfactor_before_maxgas)
self.windfactor = min(self.windfactor, self.windfactor_before_maxgas)
else:
self.gasfactor_before_maxgas = self.gasfactor
self.windfactor_before_maxgas = self.windfactor
self.gas = float(np.interp((gas_pedal_force - min_gas) * self.gasfactor + min_gas,
self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V))
# limit gas ramp to 60 units per frame, matches stock; higher sometimes makes the powertrain ignore the command
max_gas = max(60, self.bosch_last_gas + 60)
self.gas = min(self.gas, max_gas)
self.bosch_last_gas = self.gas
stopping = actuators.longControlState == LongCtrlState.stopping
self.stopping_counter = self.stopping_counter + 1 if stopping else 0
# CAN FD: never overlap the stock radar's own ACC_CONTROL stream; ours starts within a few
# frames of the radar going silent (see the deferred radar disable above)
if not (self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.stock_acc_alive):
can_sends.extend(hondacan.create_acc_commands(self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas,
self.stopping_counter, self.CP, gas_pedal_force))
else:
apply_brake = np.clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(np.clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
pcm_override = True
can_sends.append(hondacan.create_brake_command(self.packer, self.CAN, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, alert_fcw,
self.CP.carFingerprint, CS.stock_brake, self.CP_IQ))
self.apply_brake_last = apply_brake
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
gas_error = actuators.accel - CS.out.aEgo
if (not CS.out.gasPressed) and (actuators.longControlState == LongCtrlState.pid) and self.CP_IQ.enableGasInterceptor:
if gas_error != 0.0 and gas > 0.0:
self.gasfactor = np.clip(self.gasfactor + gas_error / 50 * (gas * 4.8), 0.1, 3.0)
if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0):
wind_adjust = 1 + (wind_brake * 4.8) / 1000
self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0 / wind_adjust), 0.1, 5.0)
if gas <= 0.0:
self.windfactor = max(self.windfactor, self.windfactor_before_brake)
else:
self.windfactor_before_brake = self.windfactor
can_sends.extend(GasInterceptorCarController.update(self, CC, CS, gas * self.gasfactor, brake, wind_brake, self.packer, self.frame))
# Send dashboard UI commands. On CAN FD, ACC_HUD is a radar look-alike that openpilot only owns
# once it has disabled the radar; it rides the phase-locked 10Hz hud tick instead of frame % 10
if (self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.hud_tick and
self.CP.openpilotLongitudinalControl and not CS.stock_acc_alive):
can_sends.append(hondacan.create_acc_hud(self.packer, self.CAN.pt, self.CP, CC.enabled, pcm_speed, actuators.accel,
hud_control, hud_v_cruise, CS.is_metric, CS.acc_hud))
if self.frame % 10 == 0:
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH_CANFD:
# On Nidec, this also controls longitudinal positive acceleration
can_sends.append(hondacan.create_acc_hud(self.packer, self.CAN.pt, self.CP, CC.enabled, pcm_speed, pcm_accel,
hud_control, hud_v_cruise, CS.is_metric, CS.acc_hud))
steering_available = CS.out.cruiseState.available and CS.out.vEgo > self.CP.minSteerSpeed
reduced_steering = CS.out.steeringPressed
lkas_state_change = None
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
# The key must contain exactly the signals that change the LKAS_HUD payload, nothing more:
# a flickering input (like steer saturation) re-triggers the pulse continuously, which keeps
# LKAS_STATE_CHANGE high and suppresses the dash lane lines entirely
hud_key = (bool(CC.latActive), bool(self.dashed_lanes), bool(alert_steer_required), bool(CS.out.steerFaultPermanent))
if hud_key != self.lkas_hud_key:
self.lkas_hud_key = hud_key
self.lkas_state_change_frames = 30 # 3s at the 10Hz LKAS_HUD rate, matching the stock pulse length
lkas_state_change = self.lkas_state_change_frames > 0
self.lkas_state_change_frames = max(0, self.lkas_state_change_frames - 1)
can_sends.extend(hondacan.create_lkas_hud(self.packer, self.CAN.lkas, self.CP, hud_control, CC.latActive,
steering_available, reduced_steering, alert_steer_required, CS.lkas_hud, self.dashed_lanes,
steer_fault_permanent=CS.out.steerFaultPermanent, lkas_state_change=lkas_state_change))
if self.CP.openpilotLongitudinalControl:
# TODO: combining with create_acc_hud block above will change message order and will need replay logs regenerated
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD):
can_sends.append(hondacan.create_radar_hud(self.packer, self.CAN.pt))
if self.CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH:
can_sends.append(hondacan.create_legacy_brake_command(self.packer, self.CAN.pt))
if self.CP.carFingerprint not in HONDA_BOSCH:
self.speed = pcm_speed
if not self.CP_IQ.enableGasInterceptor:
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
# Render OP's lane and lead cars on the dash. On CAN FD these are radar look-alikes that only
# exist (and are only allowed by panda safety) when the radar is disabled. Radarless keeps the
# camera as the dash authority (known-good), so OP does not author these there
if (CS.radar_50hz_tick and self.CP.carFingerprint in HONDA_BOSCH_CANFD and self.CP.openpilotLongitudinalControl
and not CS.stock_acc_alive):
leads = dash_objects.leads_from_model(self.model, CS.out.vEgo)
lead = leads[0]
lead_d = lead.dRel if lead.status else 0.0
self.rendered_lane = self.lane_renderer.update(self.model, CS.out.vEgo, lead_d)
mux = self.radar_mux
# no LKAS_HUD_2 on CAN FD: the dash reads the lane length from the in-band terminator, so the
# path is reshaped into the terminated-prefix form
lane_offsets = dash_lane.canfd_lane_offsets(self.rendered_lane)
lane_msg = dash_lane.create_lane_path(self.packer, self.CAN.lkas, lane_offsets, mux)
can_sends.append(lane_msg)
# CAN FD cars have no camera HUD_OBJECTS to poll (the disabled radar owned it): author OP's
# lead in slot 0 with the other slots blank (tracks=None)
tracks = CS.camera_object_tracker.snapshot() if CS.camera_object_tracker is not None else None
hud_msg = self.dash_object_author.create(self.packer, self.CAN.lkas, lead, tracks, mux, now_nanos * 1e-9,
extra_leads=leads[1:])
can_sends.append(hud_msg)
# the camera (behind the relay) also consumes these; mirror the identical packed bytes onto the
# camera bus (packed once, so the counter/checksum stay in lockstep)
for addr, dat, _ in (lane_msg, hud_msg):
can_sends.append((addr, dat, self.CAN.camera))
# CAN FD: when stock LKAS is active, the touch-steering-wheel nag eventually forces an ACC
# disengagement (a brake tap from the VSA). Disable LKAS automatically and block the driver's LKAS
# button by taking over SCM_BUTTONS on the camera bus while engaged (panda blocks the forwarded
# stock SCM_BUTTONS while this stream flows). Radarless keeps the stock camera LKAS untouched
if self.CP.carFingerprint in HONDA_BOSCH_CANFD and CC.enabled and self.frame % 4 == 0 and \
not pcm_cancel_cmd and not CC.cruiseControl.resume:
if self.lkas_button_send_remaining == 0 and CS.lkas_hud["LKAS_READY"] and self.frame >= self.last_lkas_button_frame + 500:
self.lkas_button_send_remaining = 3
if self.lkas_button_send_remaining > 0:
self.last_lkas_button_frame = self.frame
self.lkas_button_send_remaining -= 1
cruise_setting = CruiseSettings.LKAS
elif CS.cruise_setting == CruiseSettings.LKAS:
cruise_setting = 0 # block the driver's LKAS button press
else:
cruise_setting = CS.cruise_setting
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CS.cruise_buttons, cruise_setting,
CS.scm_ambient_light, self.CP.carFingerprint, bus=self.CAN.camera))
# Finalize actuator state for downstream consumers
new_actuators = actuators.as_builder()
new_actuators.speed = self.speed
new_actuators.accel = self.accel
new_actuators.gas = self.gas
new_actuators.brake = self.brake
new_actuators.torque = self.last_torque
new_actuators.torqueOutputCan = apply_torque
self.frame += 1
return new_actuators, can_sends

383
artifacts/package_runtime/iqdbc/car/honda/carstate.py Normal file
View File

@@ -0,0 +1,383 @@
import numpy as np
from collections import defaultdict
from iqdbc.can import CANDefine, CANParser
from iqdbc.car import Bus, create_button_events, structs, DT_CTRL
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.honda.hondacan import CanBus
from iqdbc.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_BOSCH_ALT_RADAR, HONDA_BOSCH_CANFD, \
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, HONDA_BOSCH_TJA_CONTROL, \
HondaFlags, CruiseButtons, CruiseSettings, GearShifter, CarControllerParams
from iqdbc.car.honda.dash_objects import CameraObjectTracker
from iqdbc.car.interfaces import CarStateBase
from iqdbc.lvbs.car.honda.iq_carstate import IQCarState
TransmissionType = structs.CarParams.TransmissionType
ButtonType = structs.CarState.ButtonEvent.Type
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel}
SETTINGS_BUTTONS_DICT = {CruiseSettings.DISTANCE: ButtonType.gapAdjustCruise, CruiseSettings.LKAS: ButtonType.lkas}
class CarState(CarStateBase, IQCarState):
def __init__(self, CP, CP_IQ):
CarStateBase.__init__(self, CP, CP_IQ)
IQCarState.__init__(self, CP, CP_IQ)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
if CP.transmissionType != TransmissionType.manual:
self.gearbox_msg = "GEARBOX_AUTO"
if CP.transmissionType == TransmissionType.cvt:
self.gearbox_msg = "GEARBOX_CVT"
self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]
self.car_state_scm_msg = "SCM_FEEDBACK"
if CP.carFingerprint in HONDA_NIDEC_ALT_SCM_MESSAGES:
self.car_state_scm_msg = "SCM_BUTTONS"
self.brake_error_msg = "HYBRID_BRAKE_ERROR" if CP.flags & HondaFlags.HYBRID else "STANDSTILL"
self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
self.brake_switch_prev = False
self.brake_switch_active = False
self.low_speed_alert = False
self.dynamic_v_cruise_units = self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_ALT_RADAR |
HONDA_BOSCH_TJA_CONTROL | HONDA_BOSCH_CANFD)
self.cruise_setting = 0
self.v_cruise_pcm_prev = 0
# When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster
# However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX)
self.dash_speed_seen = False
self.is_metric = False
self.v_cruise_factor = 1.
self.initial_accFault_cleared = False
self.initial_accFault_cleared_timer = int(10 / DT_CTRL) # 10 seconds after startup for initial faults to clear
self.scm_ambient_light = 0
self.radar_ref_counter = 0
self.radar_5hz_tick_counter = 0
self.radar_5hz_tick = False
self.supp_tick_counter = 0
self.supp_tick = False
self.hud_tick_counter = 0
self.hud_tick = False
self.radar_50hz_tick_counter = 0
self.radar_50hz_tick = False
# CAN FD deferred radar disable (see carcontroller): the stock radar is assumed alive until it has
# been silent for a few frames, and the relay is detected open once the camera's STEERING_CONTROL
# stops being physically visible on the PT bus
self.stock_acc_counter = 0
self.stock_acc_alive = False
self.camera_steer_counter = 0
self.camera_steer_seen = False
self.canfd_frames = 0
self.canfd_relay_open = False
# only radarless cameras emit HUD_OBJECTS to poll for adjacent-car positions; on CAN FD the
# (disabled) radar owned it, so there is nothing to track
self.camera_object_tracker = CameraObjectTracker() if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else None
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
if self.CP.enableBsm:
cp_body = can_parsers[Bus.body]
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
cp_radar = can_parsers[Bus.radar]
ret = structs.CarState()
ret_iq = structs.IQCarState()
# car params
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
prev_cruise_buttons = self.cruise_buttons
prev_cruise_setting = self.cruise_setting
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
if self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
# The camera consumes SCM_BUTTONS content beyond the buttons (losing/zeroing this byte raises an
# adaptive high beam error), so it must be echoed on frames sent in the SCM's place
self.scm_ambient_light = cp.vl["SCM_BUTTONS"]["AMBIENT_LIGHT_MAYBE"]
# used for car hud message
# TODO: find CAR_SPEED for HONDA_ODYSSEY_TWN or use ACC_HUD w/ detection
self.is_metric = self.CP.carFingerprint in (CAR.HONDA_ODYSSEY_TWN,) or not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"]
self.v_cruise_factor = CV.MPH_TO_MS if self.dynamic_v_cruise_units and not self.is_metric else CV.KPH_TO_MS
# ******************* parse out can *******************
# blend in transmission speed at low speed, since it has more low speed accuracy
# STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED
v_wheel = sum([cp.vl["WHEEL_SPEEDS"][f"WHEEL_SPEED_{s}"] for s in ("FL", "FR", "RL", "RR")]) / 4.0 * CV.KPH_TO_MS
v_weight = float(np.interp(v_wheel, v_weight_bp, v_weight_v))
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5
# doorOpen is true if we can find any door open, but signal locations vary, and we may only see the driver's door
# TODO: Test the eight Nidec cars without SCM signals for driver's door state, may be able to consolidate further
if self.CP.flags & HondaFlags.HAS_ALL_DOOR_STATES:
ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"],
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]])
elif "DRIVERS_DOOR_OPEN" in cp.vl["SCM_BUTTONS"]:
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
else:
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]]
ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
if self.CP.carFingerprint in (HONDA_BOSCH_ALT_RADAR | HONDA_BOSCH_CANFD):
# TODO: See if this logic works for all other Honda
min_steer_speed = max(CarControllerParams.STEER_GLOBAL_MIN_SPEED, self.CP.minSteerSpeed)
expected_low_speed_lockout = steer_status == "LOW_SPEED_LOCKOUT" and ret.vEgo < min_steer_speed
ret.steerFaultTemporary = steer_status != "NORMAL" and not expected_low_speed_lockout
else:
# LOW_SPEED_LOCKOUT is not worth a warning
# NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
# FIXME: the stock camera stops steering on NO_TORQUE_ALERT_1
ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "TJA_LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
# All Honda EPS cut off slightly above standstill, some much higher
# Don't alert in the near-standstill range, but alert for per-vehicle configured minimums above that
if CarControllerParams.STEER_GLOBAL_MIN_SPEED < ret.vEgo < (self.CP.minSteerSpeed + 0.5):
self.low_speed_alert = True
elif ret.vEgo > (self.CP.minSteerSpeed + 1.):
# TODO: better handle delayed steering enablement on ALT_RADAR cars
self.low_speed_alert = False
ret.lowSpeedAlert = self.low_speed_alert
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
ret.accFaulted = bool(cp.vl["CRUISE_FAULT_STATUS"]["CRUISE_FAULT"])
else:
if self.CP.openpilotLongitudinalControl:
if self.CP.carFingerprint in (HONDA_BOSCH_CANFD | HONDA_BOSCH_TJA_CONTROL) and (self.CP.flags & HondaFlags.BOSCH_ALT_BRAKE):
ret.accFaulted = bool(cp.vl["BRAKE_MODULE"]["CRUISE_FAULT"])
else:
ret.accFaulted = bool(cp.vl[self.brake_error_msg]["BRAKE_ERROR_1"] or cp.vl[self.brake_error_msg]["BRAKE_ERROR_2"])
# Log non-critical stock ACC/LKAS faults if Nidec (camera)
if self.CP.carFingerprint not in HONDA_BOSCH:
ret.carFaultedNonCritical = bool(cp_cam.vl["ACC_HUD"]["ACC_PROBLEM"] or cp_cam.vl["LKAS_HUD"]["LKAS_PROBLEM"])
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
if self.CP.carFingerprint not in (CAR.HONDA_ODYSSEY_TWN,):
self.dash_speed_seen = self.dash_speed_seen or cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] > 1e-3
if self.dash_speed_seen:
conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.vEgoCluster = cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] * conversion
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
ret.parkingBrake = bool(cp.vl[self.car_state_scm_msg]["PARKING_BRAKE_ON"])
if self.CP.transmissionType == TransmissionType.manual:
ret.gearShifter = GearShifter.reverse if bool(cp.vl["SCM_FEEDBACK"]["REVERSE_LIGHT"]) else GearShifter.drive
else:
gear_position = self.shifter_values.get(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"], None)
ret.gearShifter = self.parse_gear_shifter(gear_position)
ret.gasPressed = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] > 1e-5
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200)
if self.CP.carFingerprint in HONDA_BOSCH:
# The PCM always manages its own cruise control state, but doesn't publish it
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
ret.cruiseState.nonAdaptive = cp_cam.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"] != 0
if not self.CP.openpilotLongitudinalControl:
# ACC_HUD is on camera bus on radarless cars
acc_hud = cp_cam.vl["ACC_HUD"] if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else cp.vl["ACC_HUD"]
ret.cruiseState.nonAdaptive = acc_hud["CRUISE_CONTROL_LABEL"] != 0
ret.cruiseState.standstill = acc_hud["CRUISE_SPEED"] == 252.
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
ret.cruiseState.speed = self.v_cruise_pcm_prev if acc_hud["CRUISE_SPEED"] > 160.0 else acc_hud["CRUISE_SPEED"] * self.v_cruise_factor
self.v_cruise_pcm_prev = ret.cruiseState.speed
else:
ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS
if self.CP.flags & HondaFlags.BOSCH_ALT_BRAKE:
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
else:
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
# brake switch rises earlier than brake pressed but is never 1 when in park
brake_switch_vals = cp.vl_all["POWERTRAIN_DATA"]["BRAKE_SWITCH"]
if len(brake_switch_vals):
brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0
if len(brake_switch_vals) > 1:
self.brake_switch_prev = brake_switch_vals[-2] != 0
self.brake_switch_active = brake_switch and self.brake_switch_prev
self.brake_switch_prev = brake_switch
ret.brakePressed = (cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] != 0) or self.brake_switch_active
ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
ret.cruiseState.available = bool(cp.vl[self.car_state_scm_msg]["MAIN_ON"])
# Bosch cars can report stale ACC faults during early startup.
if ret.accFaulted:
if (self.CP.carFingerprint in HONDA_BOSCH) and not self.initial_accFault_cleared:
# Gate initial stale faults via availability (accFaulted is sticky until offroad).
ret.accFaulted = False
ret.cruiseState.available = False
elif self.initial_accFault_cleared_timer == 0:
self.initial_accFault_cleared = True
if self.initial_accFault_cleared_timer > 0:
self.initial_accFault_cleared_timer -= 1
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.HONDA_PILOT, CAR.HONDA_RIDGELINE):
if ret.brake > 0.1:
ret.brakePressed = True
if self.CP.carFingerprint in HONDA_BOSCH:
# TODO: find the radarless AEB_STATUS bit and make sure ACCEL_COMMAND is correct to enable AEB alerts
if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
else:
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
self.acc_hud = False
self.lkas_hud = False
if self.CP.carFingerprint not in HONDA_BOSCH:
ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0
self.acc_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
if self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
self.lkas_hud = cp_cam.vl["LKAS_HUD"]
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
# The radar emits low-rate tick reference messages that keep running even while its data
# messages are disabled, so the look-alikes are phased to the stock cadence off of them.
#
# There is a one-frame (10 ms) delay between reading a tick here in carstate and transmitting the
# response in carcontroller. The stock radar sends each data message in the SAME frame as its
# tick, so we pulse one frame BEFORE the next tick (counter == period-1): the +1 transmit delay
# then lands the message on the next tick frame, matching stock.
# period (frames @100Hz): 0x710=100, 0x730=10, 0x750=2, RADAR_REFERENCE=20
self.radar_ref_counter = cp.vl["RADAR_REFERENCE"]["COUNTER"]
# 5 Hz: RADAR_REFERENCE (0x3A1) is on the powertrain bus (cp), not the radar bus (cp_radar).
# RADAR_LEAD does NOT ride with the reference; stock sends it ~120 ms (12 frames) after, so fire
# at frame 11 (+1 transmit delay -> ~120 ms)
ref_tick_vals = cp.vl_all.get("RADAR_REFERENCE", {}).get("COUNTER", [])
if len(ref_tick_vals) > 0:
self.radar_5hz_tick_counter = 0
else:
self.radar_5hz_tick_counter += 1
self.radar_5hz_tick = (self.radar_5hz_tick_counter == 11)
supp_tick_vals = cp_radar.vl_all.get("RADAR_SUPP_TICK_REFERENCE", {}).get("IGNORE", [])
if len(supp_tick_vals) > 0:
self.supp_tick_counter = 0
else:
self.supp_tick_counter += 1
self.supp_tick = (self.supp_tick_counter == 99)
hud_tick_vals = cp_radar.vl_all.get("RADAR_HUD_TICK_REFERENCE", {}).get("IGNORE", [])
if len(hud_tick_vals) > 0:
self.hud_tick_counter = 0
else:
self.hud_tick_counter += 1
self.hud_tick = (self.hud_tick_counter == 9)
tick_50hz_vals = cp_radar.vl_all.get("RADAR_50HZ_TICK_REFERENCE", {}).get("IGNORE", [])
if len(tick_50hz_vals) > 0:
self.radar_50hz_tick_counter = 0
else:
self.radar_50hz_tick_counter += 1
self.radar_50hz_tick = (self.radar_50hz_tick_counter == 1)
# Deferred radar disable (see carcontroller). The stock radar transmits ACC_CONTROL every 2
# frames, so 4 missed frames means it has been silenced; assume alive until then so the
# replacement stream never overlaps it
self.canfd_frames += 1
if len(cp.vl_all.get("ACC_CONTROL", {}).get("COUNTER", [])) > 0:
self.stock_acc_counter = 0
else:
self.stock_acc_counter += 1
self.stock_acc_alive = self.stock_acc_counter < 4
# While the comma relay is closed the camera's STEERING_CONTROL is physically visible on the PT
# bus; when the relay opens it disappears (openpilot's own 0xE4 TX is not parsed as RX). As a
# fallback, assume the relay is open after 5 s of controls in case the camera was never seen
if len(cp.vl_all.get("STEERING_CONTROL", {}).get("COUNTER", [])) > 0:
self.camera_steer_counter = 0
self.camera_steer_seen = True
else:
self.camera_steer_counter += 1
self.canfd_relay_open = (self.camera_steer_seen and self.camera_steer_counter >= 5) or self.canfd_frames >= 500
else:
self.supp_tick = False
self.hud_tick = False
self.radar_5hz_tick = False
self.radar_50hz_tick = False
if self.CP.enableBsm:
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
# more info here: https://github.com/commaai/openpilot/pull/1867
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
ret.buttonEvents = [
*create_button_events(self.cruise_buttons, prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(self.cruise_setting, prev_cruise_setting, SETTINGS_BUTTONS_DICT),
]
IQCarState.update(self, ret, ret_iq, can_parsers)
if self.camera_object_tracker is not None:
self.camera_object_tracker.update(cp_cam)
return ret, ret_iq
def get_can_parsers(self, CP, CP_IQ):
pt_messages = []
cam_messages = []
if CP.carFingerprint in HONDA_BOSCH_CANFD:
# Radar-alive and relay-open detection for the deferred radar disable (see carcontroller).
# Both messages intentionally go silent (the radar is disabled, the camera ends up behind the
# open relay), so subscribe with NaN frequency to skip the alive/timeout checks
pt_messages += [("ACC_CONTROL", float('nan')), ("STEERING_CONTROL", float('nan'))]
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
# polled by the CameraObjectTracker, but not every radarless camera emits it
cam_messages += [("HUD_OBJECTS", float('nan'))]
parsers = {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus(CP).pt),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus(CP).camera),
}
if CP.enableBsm:
parsers[Bus.body] = CANParser(DBC[CP.carFingerprint][Bus.body], [], CanBus(CP).radar)
if CP.carFingerprint in HONDA_BOSCH_CANFD:
# The tick references are only read via vl_all, which (unlike vl) does not auto-subscribe
# messages, so they must be listed explicitly or they are never parsed.
# 0x710 RADAR_SUPP_TICK_REFERENCE (1 Hz), 0x730 RADAR_HUD_TICK_REFERENCE (10 Hz),
# 0x750 RADAR_50HZ_TICK_REFERENCE (50 Hz)
parsers[Bus.radar] = CANParser(DBC[CP.carFingerprint][Bus.radar], [
("RADAR_SUPP_TICK_REFERENCE", 0),
("RADAR_HUD_TICK_REFERENCE", 0),
("RADAR_50HZ_TICK_REFERENCE", 0),
], CanBus(CP).radar)
return parsers

186
artifacts/package_runtime/iqdbc/car/honda/dash_lane.py Normal file
View File

@@ -0,0 +1,186 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from dataclasses import dataclass, field
import numpy as np
POINT_COUNT = 40
POINTS_PER_FRAME = 4
SWEEP_INDICES = POINT_COUNT // POINTS_PER_FRAME
# the camera repeats each sweep index across four redundant banks: mux = index + bank*16,
# giving mux values 1-10, 17-26, 33-42 and 49-58 for logical indices 0-9
MUX_CYCLE = tuple(index + bank * 16 for bank in range(4) for index in range(1, SWEEP_INDICES + 1))
OFFSET_UNAVAILABLE = 2047
OFFSET_VALID_MAX = 2046
NEAR_M = 2.0
FAR_M = 100.0
LOOKAHEAD_M = np.linspace(NEAR_M, FAR_M, POINT_COUNT)
# full swing center -> max turn is slewed over this long so model jumps can't teleport the dash lane
SLEW_RATE_HZ = 50.0
SLEW_FULL_SCALE_S = 2.0
SLEW_MAX_STEP = OFFSET_VALID_MAX / (SLEW_FULL_SCALE_S * SLEW_RATE_HZ)
def _stock_gain(d):
# raw offset units per meter of lateral, regressed from stock radar sweeps vs modelV2 lane centers
return 29.3 + 0.243 * d - 0.00228 * d ** 2
def _legacy_gain(d):
return 6.27 + 0.0106 * d + 0.000354 * d ** 2
GAIN = _stock_gain(LOOKAHEAD_M)
def gain_correction(d: float) -> float:
# the HUD lead marker's lateral scale was tuned against lanes drawn with the legacy (flatter) gain
# law, so the lead's lateral must ride this ratio to stay on the corrected lane rendering
d = min(max(float(d), NEAR_M), FAR_M)
return _stock_gain(d) / _legacy_gain(d)
LANE_LINE_ON = 3
LANE_LENGTH_MAX_VALUE = 33
LANE_WIDTH_DEFAULT = 32
LINE_PROB_ON = 0.25
LINE_PROB_OFF = 0.10
HALF_LANE_M = 1.65
FULL_REACH_SPEED = 27.0
FULL_REACH_LEAD_DIST = 70.0
MIN_REACH = 0.15
def encode_lane_path(x, y):
x = np.asarray(x, dtype=float)
y = np.asarray(y, dtype=float)
if x.size < 2 or x.max() < FAR_M:
return [OFFSET_UNAVAILABLE] * POINT_COUNT
lat = np.interp(LOOKAHEAD_M, x, y)
# stock encodes offsets with the opposite lateral sign to openpilot's +left convention
raw = np.clip(np.round(-GAIN * lat), -OFFSET_VALID_MAX, OFFSET_VALID_MAX)
return [int(v) for v in raw]
# The CAN FD dash has no LKAS_HUD_2 to carry the drawn length: it reads the path as a contiguous valid
# prefix ended by an in-band OFFSET_UNAVAILABLE terminator, idles at 6 valid zero offsets (never
# all-unavailable), and cross-checks the prefix length against RADAR_LEAD's LANE_PATH_LENGTH.
CANFD_MAX_VALID_PTS = 23
CANFD_MIN_VALID_PTS = 6
CANFD_IDLE_OFFSETS = [0] * CANFD_MIN_VALID_PTS + [OFFSET_UNAVAILABLE] * (POINT_COUNT - CANFD_MIN_VALID_PTS)
# stock valid-point count is a function of ego speed alone, fit from factory lanes-on RADAR_LEAD frames
CANFD_LEN_INTERCEPT = 6.74
CANFD_LEN_SLOPE = 0.862
@dataclass
class RenderedLane:
offsets: list[int] = field(default_factory=lambda: [OFFSET_UNAVAILABLE] * POINT_COUNT)
reach: float = 0.0
left_line: bool = False
right_line: bool = False
lane_cross: int = 0
v_ego: float = 0.0
@property
def blank(self) -> bool:
return self.reach <= 0.0 or self.offsets[0] == OFFSET_UNAVAILABLE
def canfd_lane_length(lane: RenderedLane) -> int:
if lane.blank:
return CANFD_MIN_VALID_PTS
n = round(CANFD_LEN_INTERCEPT + CANFD_LEN_SLOPE * lane.v_ego)
return max(CANFD_MIN_VALID_PTS, min(CANFD_MAX_VALID_PTS, n))
def canfd_lane_offsets(lane: RenderedLane) -> list[int]:
if lane.blank:
return CANFD_IDLE_OFFSETS
n_valid = canfd_lane_length(lane)
return list(lane.offsets[:n_valid]) + [OFFSET_UNAVAILABLE] * (POINT_COUNT - n_valid)
def create_lane_path(packer, bus, offsets, mux):
base = ((mux - 1) % 16) * POINTS_PER_FRAME
values = {"MUX": mux}
for i in range(POINTS_PER_FRAME):
values[f"PATH_OFFSET_{i + 1}"] = offsets[base + i]
return packer.make_can_msg("LANE_PATH", bus, values)
def create_lkas_hud_2(packer, bus, counter_2, reach=1.0, lane_cross=0, left_line=True, right_line=True):
lane_length = max(0, min(LANE_LENGTH_MAX_VALUE, round(reach * LANE_LENGTH_MAX_VALUE)))
shown = lane_length > 0
values = {
"COUNTER_2": counter_2,
"SET_ME_X01": 1,
"LANE_WIDTH": LANE_WIDTH_DEFAULT,
"LEFT_LANE": LANE_LINE_ON if (shown and left_line) else 0,
"RIGHT_LANE": LANE_LINE_ON if (shown and right_line) else 0,
"LEFT_LANE_CROSSED": 1 if (shown and lane_cross < 0) else 0,
"RIGHT_LANE_CROSSED": 1 if (shown and lane_cross > 0) else 0,
"LANE_LENGTH": lane_length,
}
return packer.make_can_msg("LKAS_HUD_2", bus, values)
class LanePathRenderer:
def __init__(self):
self._left_on = False
self._right_on = False
self._shown = None
def _lane_center(self, model):
lls, probs = model.laneLines, model.laneLineProbs
if len(lls) < 3 or len(probs) < 3 or len(lls[1].x) == 0:
return None, None, False, False
left = probs[1] >= (LINE_PROB_OFF if self._left_on else LINE_PROB_ON)
right = probs[2] >= (LINE_PROB_OFF if self._right_on else LINE_PROB_ON)
x = np.array(lls[1].x)
yl, yr = np.array(lls[1].y), np.array(lls[2].y)
if left and right:
y = (yl + yr) / 2.0
elif right:
y = yr - HALF_LANE_M
elif left:
y = yl + HALF_LANE_M
else:
return None, None, False, False
return x, y, left, right
def _slew(self, offsets):
# an all-sentinel fit draws nothing: pass through and reset so the next real fit shows unslewed
if offsets[0] == OFFSET_UNAVAILABLE:
self._shown = None
return offsets
target = np.asarray(offsets, dtype=float)
if self._shown is None:
self._shown = target
else:
self._shown = self._shown + np.clip(target - self._shown, -SLEW_MAX_STEP, SLEW_MAX_STEP)
return [int(v) for v in np.round(self._shown)]
def update(self, model, v_ego, lead_d) -> RenderedLane:
x = y = None
left_on = right_on = False
if model is not None:
x, y, left_on, right_on = self._lane_center(model)
if x is None:
self._shown = None
return RenderedLane()
self._left_on, self._right_on = left_on, right_on
reach = float(np.clip(max(v_ego / FULL_REACH_SPEED, lead_d / FULL_REACH_LEAD_DIST, MIN_REACH), 0.0, 1.0))
if round(reach * LANE_LENGTH_MAX_VALUE) <= 0:
self._shown = None
return RenderedLane()
return RenderedLane(self._slew(encode_lane_path(x, y)), reach, left_on, right_on, v_ego=v_ego)

314
artifacts/package_runtime/iqdbc/car/honda/dash_objects.py Normal file
View File

@@ -0,0 +1,314 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
from dataclasses import dataclass
from iqdbc.can.parser import CANParser
from iqdbc.car.honda import dash_lane
NUM_SLOTS = 10
LONG_DIST_CAP_M = 195.0
# byte-faithful empty-slot payload decoded from stock HUD_OBJECTS; an inconsistent frame risks the dash rejecting it
INACTIVE = {
"OBJECT_ID": 0,
"IS_LEAD_CAR": 0,
"CAR_TYPE": -1,
"ROTATION": -128,
"LONG_DIST": 196.9,
"LAT_DIST": 204.7,
}
CAR_TYPE_CAR = 7
LONG_DIST_MAX_M = 194.0
LAT_DIST_LIM_M = 204.7
# the dash under-scales LAT_DIST ~0.3x in the ego frame; tuned on-car so the lead marker lands on the lane
LAT_SCALE = 0.35
ROT_BAND_M = 1.5
ROT_MAX = 6
REID_GAP_M = 8.0
REID_TAU = 1.5
REID_REFRACTORY = 1.5
MAX_OBJECT_ID = 31
DREL_SMOOTH_TAU = 0.6
YREL_SMOOTH_TAU = 0.5
FF_VREL_MIN = 0.5
DREL_RESID_CLAMP = 1.5
LEAD_PROB_ON = 0.5
LEAD_PROB_OFF = 0.35
LEAD_HOLD_S = 0.6
# modelV2.leadsV3 entries are one car at three time horizons, not three cars: only render the extra
# horizons when spatially distinct from everything already rendered (a genuinely different vehicle)
EXTRA_LEAD_SLOTS = (1, 2)
EXTRA_LEAD_MIN_SEP_D = 5.0
EXTRA_LEAD_MIN_SEP_Y = 1.5
@dataclass
class CameraObject:
slot: int
object_id: int
d_rel: float
y_rel: float
is_lead_car: bool
valid: bool
car_type: int = -1
rotation: int = -128
class CameraObjectTracker:
def __init__(self):
self._tracks: list[CameraObject] = [
CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
for i in range(NUM_SLOTS)
]
def update(self, cp_cam: CANParser) -> None:
vla = cp_cam.vl_all["HUD_OBJECTS"]
for mux, oid, ld, yd, lead, ct, rot in zip(vla["MUX"], vla["OBJECT_ID"], vla["LONG_DIST"], vla["LAT_DIST"],
vla["IS_LEAD_CAR"], vla["CAR_TYPE"], vla["ROTATION"], strict=True):
slot = (int(mux) - 1) % 16
if 0 <= slot < NUM_SLOTS:
self._tracks[slot] = CameraObject(
slot=slot,
object_id=int(oid),
d_rel=float(ld),
y_rel=float(yd),
is_lead_car=bool(lead),
valid=oid != 0 and ld < LONG_DIST_CAP_M,
car_type=int(ct),
rotation=int(rot),
)
def snapshot(self) -> list[CameraObject]:
return self._tracks
@dataclass
class ModelLead:
status: bool
dRel: float
yRel: float
vRel: float
prob: float = 0.0
def leads_from_model(model, v_ego, n=3):
# modelV2's lateral is +right; the dash convention is +left. v is made relative for the smoother.
# Data stays populated below LEAD_PROB_ON (status False, prob carried) so the author's hysteresis
# can keep an already-rendered lead alive down to LEAD_PROB_OFF instead of blinking it
out = []
for i in range(n):
if model is None or len(model.leadsV3) <= i or len(model.leadsV3[i].x) == 0:
out.append(ModelLead(False, 0.0, 0.0, 0.0))
continue
lead = model.leadsV3[i]
out.append(ModelLead(bool(lead.prob >= LEAD_PROB_ON), float(lead.x[0]), -float(lead.y[0]),
float(lead.v[0]) - v_ego, prob=float(lead.prob)))
return out
def lead_rotation(lateral_left_m: float) -> int:
magnitude = min(round(abs(lateral_left_m) / ROT_BAND_M), ROT_MAX)
return -magnitude if lateral_left_m > 0 else magnitude
class LeadIdentity:
"""Mints a stable OBJECT_ID for the rendered lead, re-IDing on a fresh lead or a range discontinuity.
dRel is noisy, so a leaky predictor (feed-forward vRel, leak toward dRel) accumulates the residual
instead of a per-sample range-rate test."""
def __init__(self):
self.object_id = 0
self._on = False
self._pred = 0.0
self._prev_t = 0.0
self._reid_t = -1e9
def update(self, status: bool, d_rel: float, v_rel: float, now: float) -> int:
if not status:
self.object_id = 0
self._on = False
return 0
new_lead = not self._on
if self._on:
dt = max(now - self._prev_t, 1e-3)
self._pred += v_rel * dt
self._pred += min(dt / REID_TAU, 1.0) * (d_rel - self._pred)
if abs(d_rel - self._pred) > REID_GAP_M and now - self._reid_t > REID_REFRACTORY:
new_lead = True
self._prev_t = now
if new_lead:
self.object_id = self.object_id % MAX_OBJECT_ID + 1
self._reid_t = now
self._pred = d_rel
self._on = True
return self.object_id
class MarkerSmoother:
"""Stabilizes a rendered marker without lagging real motion: vRel feed-forward on dRel with a
clamped leak toward the measurement, plain low-pass on yRel, snapping on an identity change."""
def __init__(self):
self._id = 0
self._d = 0.0
self._y = 0.0
self._t = 0.0
def update(self, d_rel: float, y_rel: float, v_rel: float, object_id: int, now: float) -> tuple[float, float]:
if object_id != self._id:
self._id, self._d, self._y, self._t = object_id, d_rel, y_rel, now
return d_rel, y_rel
dt = max(now - self._t, 1e-3)
self._t = now
if abs(v_rel) >= FF_VREL_MIN:
self._d += v_rel * dt
resid = min(max(d_rel - self._d, -DREL_RESID_CLAMP), DREL_RESID_CLAMP)
self._d += (1.0 - math.exp(-dt / DREL_SMOOTH_TAU)) * resid
self._y += (1.0 - math.exp(-dt / YREL_SMOOTH_TAU)) * (y_rel - self._y)
return self._d, self._y
def create_hud_object(packer, bus, mux, track):
values = {"MUX": mux}
if track is None:
values.update(INACTIVE)
else:
values.update({
"OBJECT_ID": int(track["object_id"]),
"IS_LEAD_CAR": int(track["is_lead_car"]),
"CAR_TYPE": int(track["car_type"]),
"ROTATION": int(track["rotation"]),
"LONG_DIST": min(max(track["d_rel"], 0.0), LONG_DIST_MAX_M),
"LAT_DIST": min(max(track["y_rel"], -LAT_DIST_LIM_M), LAT_DIST_LIM_M),
})
return packer.make_can_msg("HUD_OBJECTS", bus, values)
def forward_hud_object(packer, bus, mux, tracks):
slot = (mux - 1) % 16
st = tracks[slot] if (tracks and slot < len(tracks)) else None
track = ({"d_rel": st.d_rel, "y_rel": st.y_rel, "object_id": st.object_id, "is_lead_car": st.is_lead_car,
"car_type": st.car_type, "rotation": st.rotation} if (st is not None and st.valid) else None)
return create_hud_object(packer, bus, mux, track)
class DashObjectAuthor:
"""Authors HUD_OBJECTS: openpilot's lead in slot 0 with a stable identity and smoothed marker, the
camera's non-lead cars forwarded in slots 1-9 (or distinct extra model leads where there is no
camera to forward), one frame per mux tick."""
def __init__(self):
self._identity = LeadIdentity()
self._smoother = MarkerSmoother()
self._lead_id = 0
self._prev_op_id = 0
self._lead_on = False
self._lead_hold: ModelLead | None = None
self._lead_seen_t = -1e9
self._extra_ids = {slot: LeadIdentity() for slot in EXTRA_LEAD_SLOTS}
self._extra_smooth = {slot: MarkerSmoother() for slot in EXTRA_LEAD_SLOTS}
self._extra_emit = dict.fromkeys(EXTRA_LEAD_SLOTS, 0)
def _gate_lead(self, lead: ModelLead, now: float) -> ModelLead:
# leadsV3[0].prob hovers around 0.5 in traffic; hysteresis plus a short dead-reckoned hold keeps
# the marker from blinking at a cadence the stock radar never produces
if lead.prob >= (LEAD_PROB_OFF if self._lead_on else LEAD_PROB_ON):
self._lead_on = True
self._lead_hold = lead
self._lead_seen_t = now
return lead if lead.status else ModelLead(True, lead.dRel, lead.yRel, lead.vRel, lead.prob)
if self._lead_on and self._lead_hold is not None and now - self._lead_seen_t < LEAD_HOLD_S:
h = self._lead_hold
return ModelLead(True, h.dRel + h.vRel * (now - self._lead_seen_t), h.yRel, h.vRel, h.prob)
self._lead_on = False
self._lead_hold = None
return ModelLead(False, 0.0, 0.0, 0.0)
def _lead_object_id(self, status: bool, op_id: int, stock_lead_id: int | None, in_use: set[int]) -> int:
if not status:
self._lead_id = 0
elif stock_lead_id is not None:
self._lead_id = stock_lead_id
elif self._lead_id == 0 or op_id != self._prev_op_id or self._lead_id in in_use:
# advance from the current id rather than picking the lowest free one: with no camera ids in
# use a handoff would keep the same id and the id-keyed smoother would slide between two cars
# instead of snapping
nxt = self._lead_id % MAX_OBJECT_ID + 1
while nxt in in_use:
nxt = nxt % MAX_OBJECT_ID + 1
self._lead_id = nxt
self._prev_op_id = op_id
return self._lead_id
def _update_extras(self, extra_leads, lead, in_use, now):
rendered = [(lead.dRel, lead.yRel)] if lead.status else []
out = {}
for slot, ex in zip(EXTRA_LEAD_SLOTS, extra_leads or (), strict=False):
distinct = ex.status and all(abs(ex.dRel - d) >= EXTRA_LEAD_MIN_SEP_D or
abs(ex.yRel - y) >= EXTRA_LEAD_MIN_SEP_Y
for d, y in rendered)
op_id = self._extra_ids[slot].update(distinct, ex.dRel, ex.vRel, now)
if not distinct:
self._extra_emit[slot] = 0
out[slot] = None
continue
emit = self._extra_emit[slot]
if emit == 0 or emit in in_use:
emit = op_id
while emit in in_use:
emit = emit % MAX_OBJECT_ID + 1
self._extra_emit[slot] = emit
in_use.add(emit)
d_rel, y_rel = self._extra_smooth[slot].update(ex.dRel, LAT_SCALE * ex.yRel, ex.vRel, emit, now)
rendered.append((ex.dRel, ex.yRel))
out[slot] = {"d_rel": d_rel, "y_rel": y_rel, "object_id": emit, "is_lead_car": 0,
"car_type": CAR_TYPE_CAR, "rotation": lead_rotation(y_rel / LAT_SCALE)}
return out
def create(self, packer, bus, lead, tracks, mux: int, now: float, extra_leads=None):
lead = self._gate_lead(lead, now)
op_id = self._identity.update(lead.status, lead.dRel, lead.vRel, now)
stock_lead, in_use = None, set()
for t in (tracks or ()):
if not t.valid:
continue
if t.is_lead_car:
stock_lead = t
elif t.slot != 0:
in_use.add(t.object_id)
stock_lead_id = stock_lead.object_id if stock_lead is not None else None
lead_id = self._lead_object_id(lead.status, op_id, stock_lead_id, in_use)
if lead.status:
in_use.add(lead_id)
# ride the lane gain-law correction at the lead's distance so the marker tracks the lane rendering
lat_scale = LAT_SCALE * dash_lane.gain_correction(lead.dRel)
d_rel, y_rel = self._smoother.update(lead.dRel, lat_scale * lead.yRel, lead.vRel, lead_id, now)
extras = self._update_extras(extra_leads, lead, in_use, now) if tracks is None else {}
slot = (mux - 1) % 16
if slot == 0 and lead.status:
track = {"d_rel": d_rel, "y_rel": y_rel, "object_id": lead_id, "is_lead_car": 1,
"car_type": stock_lead.car_type if stock_lead is not None else CAR_TYPE_CAR,
"rotation": stock_lead.rotation if stock_lead is not None else lead_rotation(y_rel / lat_scale)}
elif slot in extras:
track = extras[slot]
else:
st = tracks[slot] if (tracks and slot < len(tracks)) else None
# never forward the camera's lead: if OP has no lead, the HUD must not flag one OP isn't acting on
track = ({"d_rel": st.d_rel, "y_rel": st.y_rel, "object_id": st.object_id, "is_lead_car": 0,
"car_type": st.car_type, "rotation": st.rotation}
if (st is not None and st.valid and not st.is_lead_car) else None)
return create_hud_object(packer, bus, mux, track)

1102
artifacts/package_runtime/iqdbc/car/honda/fingerprints.py Normal file
View File

File diff suppressed because it is too large Load Diff

324
artifacts/package_runtime/iqdbc/car/honda/hondacan.py Normal file
View File

@@ -0,0 +1,324 @@
from iqdbc.car import CanBusBase
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.honda.values import (HondaFlags, HONDA_BOSCH, HONDA_BOSCH_ALT_RADAR, HONDA_BOSCH_RADARLESS,
HONDA_BOSCH_CANFD, CarControllerParams)
from iqdbc.lvbs.car.honda.iq_values import HondaFlagsIQ
# CAN bus layout with relay
# 0 = ACC-CAN - radar side
# 1 = F-CAN B - powertrain
# 2 = ACC-CAN - camera side
# 3 = F-CAN A - OBDII port
class CanBus(CanBusBase):
def __init__(self, CP=None, fingerprint=None) -> None:
# use fingerprint if specified
super().__init__(CP if fingerprint is None else None, fingerprint)
# powertrain bus is split instead of radar on radarless and CAN FD Bosch
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD):
self._pt, self._radar = self.offset + 1, self.offset
# normally steering commands are sent to radar, which forwards them to powertrain bus
# when radar is disabled, steering commands are sent directly to powertrain bus
self._lkas = self._pt if CP.openpilotLongitudinalControl else self._radar
else:
self._pt, self._radar, self._lkas = self.offset, self.offset + 1, self.offset
@property
def pt(self) -> int:
return self._pt
@property
def radar(self) -> int:
return self._radar
@property
def camera(self) -> int:
return self.offset + 2
@property
def lkas(self) -> int:
return self._lkas
# B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port)
@property
def body(self) -> int:
return self.offset
def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake, CP_IQ):
# TODO: do we loose pressure if we keep pump off for long?
brakelights = apply_brake > 0
brake_rq = apply_brake > 0
pcm_fault_cmd = False
values = {
"CRUISE_OVERRIDE": pcm_override,
"CRUISE_FAULT_CMD": pcm_fault_cmd,
"CRUISE_CANCEL_CMD": pcm_cancel_cmd,
"COMPUTER_BRAKE_REQUEST": brake_rq,
"SET_ME_1": 1,
"BRAKE_LIGHTS": brakelights,
"CHIME": stock_brake["CHIME"] if fcw else 0, # send the chime for stock fcw
"FCW": fcw << 1, # TODO: Why are there two bits for fcw?
"AEB_REQ_1": 0,
"AEB_REQ_2": 0,
"AEB_STATUS": 0,
}
if CP_IQ.flags & HondaFlagsIQ.NIDEC_HYBRID:
values["COMPUTER_BRAKE_HYBRID"] = apply_brake
values["BRAKE_PUMP_REQUEST_HYBRID"] = apply_brake > 0
else:
values["COMPUTER_BRAKE"] = apply_brake
values["BRAKE_PUMP_REQUEST"] = pump_on
return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values)
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, CP, gas_force):
commands = []
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
control_on = 5 if enabled else 0
gas_command = gas if active and gas_force > min_gas_accel else -30000
accel_command = accel if active else 0
braking = 1 if active and gas_force < min_gas_accel else 0
standstill = 1 if active and stopping_counter > 0 else 0
standstill_release = 1 if active and stopping_counter == 0 else 0
# common ACC_CONTROL values
acc_control_values = {
'ACCEL_COMMAND': accel_command,
'STANDSTILL': standstill,
}
if CP.flags & HondaFlags.BOSCH_RADARLESS:
acc_control_values.update({
"CONTROL_ON": enabled,
# hybrid and alt-brake cars require this bit whenever braking; others use it for idle stop after 4s at 50Hz
"COMPUTER_BRAKE_ASSIST": braking if CP.flags & (HondaFlags.HYBRID | HondaFlags.BOSCH_ALT_BRAKE) else stopping_counter > 200,
})
else:
acc_control_values.update({
'BRAKE_REQUEST': braking,
# setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1
"CONTROL_ON": control_on,
"GAS_COMMAND": gas_command, # used for gas
"BRAKE_LIGHTS": braking,
"STANDSTILL_RELEASE": standstill_release,
})
acc_control_on_values = {
"SET_TO_3": 0x03,
"CONTROL_ON": enabled,
"SET_TO_FF": 0xff,
"SET_TO_75": 0x75,
"SET_TO_30": 0x30,
}
commands.append(packer.make_can_msg("ACC_CONTROL_ON", CAN.pt, acc_control_on_values))
commands.append(packer.make_can_msg("ACC_CONTROL", CAN.pt, acc_control_values))
return commands
def create_steering_control(packer, CAN, apply_torque, lkas_active, tja_control):
values = {
"STEER_TORQUE": apply_torque if lkas_active else 0,
"STEER_TORQUE_REQUEST": lkas_active,
}
if tja_control:
values["STEER_DOWN_TO_ZERO"] = lkas_active
return packer.make_can_msg("STEERING_CONTROL", CAN.lkas, values)
def create_bosch_supplemental_1(packer, CAN):
# non-active params
values = {
"SET_ME_X04": 0x04,
"SET_ME_X80": 0x80,
"SET_ME_X10": 0x10,
}
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", CAN.lkas, values)
def create_acc_hud(packer, bus, CP, enabled, pcm_speed, pcm_accel, hud_control, hud_v_cruise, is_metric, acc_hud):
acc_hud_values = {
'CRUISE_SPEED': hud_v_cruise,
'ENABLE_MINI_CAR': 1 if enabled else 0,
# only moves the lead car without ACC_ON
'HUD_DISTANCE': hud_control.leadDistanceBars, # wraps to 0 at 4 bars
'IMPERIAL_UNIT': int(not is_metric),
'HUD_LEAD': 2 if enabled and hud_control.leadVisible else 1 if enabled else 0,
'SET_ME_X01_2': 1,
}
if CP.flags & HondaFlags.BOSCH_CANFD:
acc_hud_values['SET_ME_X01'] = int(enabled and (bool(acc_hud_values['HUD_LEAD']) or (pcm_accel < 0.2)))
acc_hud_values['SET_ME_X01_2'] = int(enabled and (bool(acc_hud_values['HUD_LEAD']) or (pcm_accel < 0.2)))
if CP.carFingerprint in HONDA_BOSCH:
acc_hud_values['ACC_ON'] = int(enabled)
acc_hud_values['FCM_OFF'] = 0
acc_hud_values['FCM_OFF_2'] = 0
else:
# Shows the distance bars, TODO: stock camera shows updates temporarily while disabled
acc_hud_values['ACC_ON'] = int(enabled)
acc_hud_values['PCM_SPEED'] = pcm_speed * CV.MS_TO_KPH
acc_hud_values['PCM_GAS'] = pcm_accel
acc_hud_values['SET_ME_X01'] = 1
acc_hud_values['FCM_OFF'] = acc_hud['FCM_OFF']
acc_hud_values['FCM_OFF_2'] = acc_hud['FCM_OFF_2']
acc_hud_values['FCM_PROBLEM'] = acc_hud['FCM_PROBLEM']
acc_hud_values['ICONS'] = acc_hud['ICONS']
return packer.make_can_msg("ACC_HUD", bus, acc_hud_values)
def create_lkas_hud(packer, bus, CP, hud_control, lat_active, steering_available, reduced_steering, alert_steer_required, lkas_hud, dashed_lanes,
steer_fault_permanent=False, lkas_state_change=None):
commands = []
lkas_hud_values = {
'LKAS_READY': 1,
'LKAS_STATE_CHANGE': 1,
'STEERING_REQUIRED': alert_steer_required,
'SOLID_LANES': lat_active,
'DASHED_LANES': dashed_lanes,
'BEEP': 0,
}
# the stock camera holds LKAS_STATE_CHANGE low, pulsing it high ~3s around HUD state changes;
# holding it high permanently suppresses the dash lane-line rendering
if lkas_state_change is not None:
lkas_hud_values['LKAS_STATE_CHANGE'] = int(lkas_state_change)
if CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
lkas_hud_values['LANE_LINES'] = 3
lkas_hud_values['DASHED_LANES'] = lat_active
# car likely needs to see LKAS_PROBLEM fall within a specific time frame, so forward from camera
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
lkas_hud_values['LKAS_PROBLEM'] = lkas_hud['LKAS_PROBLEM']
if CP.carFingerprint in HONDA_BOSCH_CANFD:
lkas_hud_values['LKAS_PROBLEM'] = steer_fault_permanent
# CAN FD: dashed lanes are the AOL armed indication (dashed_lanes is aol.enabled and not
# latActive, which is not standstill-gated - so parked LKAS button presses produce cluster
# feedback). ORed with lat_active so the engaged payload keeps SOLID and DASHED set together,
# byte-matching the stock camera's lanes-on state
lkas_hud_values['DASHED_LANES'] = dashed_lanes or lat_active
# every payload change must coincide with an LKAS_STATE_CHANGE pulse (see carcontroller); keyed
# on lat_active, not lanesVisible, so the dash LKAS indication follows AOL's lateral state
lkas_hud_values['SOLID_LANES'] = lat_active
if not (CP.flags & HondaFlags.BOSCH_EXT_HUD):
lkas_hud_values['RDM_OFF'] = 1
lkas_hud_values['LANE_ASSIST_BEEP_OFF'] = 1
# New HUD concept for selected Bosch cars, overwrites some of the above
# TODO: make global across all Honda if feedback is favorable
if CP.carFingerprint in HONDA_BOSCH_ALT_RADAR:
lkas_hud_values['DASHED_LANES'] = steering_available and lat_active
lkas_hud_values['SOLID_LANES'] = lat_active
lkas_hud_values['LKAS_PROBLEM'] = lat_active and reduced_steering
if CP.flags & HondaFlags.BOSCH_EXT_HUD and not CP.openpilotLongitudinalControl:
commands.append(packer.make_can_msg('LKAS_HUD_A', bus, lkas_hud_values))
commands.append(packer.make_can_msg('LKAS_HUD_B', bus, lkas_hud_values))
else:
commands.append(packer.make_can_msg('LKAS_HUD', bus, lkas_hud_values))
return commands
def create_radar_hud(packer, bus):
radar_hud_values = {
'CMBS_OFF': 0x01,
'SET_TO_1': 0x01,
}
return packer.make_can_msg('RADAR_HUD', bus, radar_hud_values)
def create_legacy_brake_command(packer, bus):
return packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus, {})
def spam_buttons_command(packer, CAN, cruise_button, cruise_setting, ambient_light, car_fingerprint, bus=None):
values = {
'CRUISE_BUTTONS': cruise_button,
'CRUISE_SETTING': cruise_setting,
# the camera consumes this byte too (adaptive high beam); echo the SCM's live value
'AMBIENT_LIGHT_MAYBE': ambient_light,
}
if bus is None:
# send buttons to camera on radarless (camera does ACC) cars
bus = CAN.camera if car_fingerprint in HONDA_BOSCH_RADARLESS else CAN.pt
return packer.make_can_msg("SCM_BUTTONS", bus, values)
def create_radar_hud_canfd(packer, bus, acc, acc_pulse=False):
values = {
# the stock radar raises this bit only in short bursts right after ACC engages, never held
'CMBS_ENABLED_MAYBE': 1 if (acc and acc_pulse) else 0,
'ACC_ON': acc,
'SET_ME_X01': 0x01,
'SET_ME_X01_2': 0x01,
}
return packer.make_can_msg("RADAR_HUD_CANFD", bus, values)
def create_canfd_supplemental(packer, bus):
values = {
'SET_ME_X01': 0x01,
'SET_ME_X41': 0x41,
}
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_CANFD", bus, values)
def create_canfd_5hz_radar_messages(packer, bus, radar_ref_cntr, lane_path_length=6, left_lane=0, right_lane=0):
commands = []
radar_lead_values = {
'CNTR_REF': radar_ref_cntr,
'SET_ME_X01': 0x01,
# stock radar transmits a constant 140 here; 120 causes a camera mismatch
'TARGET_SPEED_MAYBE': 140,
'LEFT_LANE': left_lane,
'RIGHT_LANE': right_lane,
# the dash cross-checks this against the LANE_PATH in-band terminator; a mismatch suppresses the lane lines
'LANE_PATH_LENGTH': lane_path_length,
}
commands.append(packer.make_can_msg('RADAR_LEAD', bus, radar_lead_values))
radar_lead2_values = {
'SET_ME_X88': 136,
'SET_ME_X78': 120,
'LEAD_DISTANCE_MAYBE': 0,
}
commands.append(packer.make_can_msg('RADAR_LEAD2', bus, radar_lead2_values))
return commands
def honda_checksum(address: int, sig, d: bytearray) -> int:
s = 0
extended = address > 0x7FF
# extended ids above 0x100000 use a different checksum constant, observed on Bosch CAN FD radar messages
high_extended = address > 0x100000
addr = address
while addr:
s += addr & 0xF
addr >>= 4
for i in range(len(d)):
x = d[i]
if i == len(d) - 1:
x >>= 4
s += (x & 0xF) + (x >> 4)
s = 8 - s
if extended:
s += 10 if high_extended else 3
return s & 0xF

392
artifacts/package_runtime/iqdbc/car/honda/interface.py Normal file
View File

@@ -0,0 +1,392 @@
#!/usr/bin/env python3
import numpy as np
from iqdbc.car import get_safety_config, structs, uds
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.disable_ecu import disable_ecu, clear_all_dtcs, clear_ecu_dtcs
from iqdbc.car.honda.hondacan import CanBus
from iqdbc.car.honda.values import CarControllerParams, HondaFlags, CAR, HONDA_BOSCH, HONDA_BOSCH_CANFD, \
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, \
HONDA_RADAR_SCAN_VERIFIED, HondaSafetyFlags
from iqdbc.car.honda.carcontroller import CarController
from iqdbc.car.honda.carstate import CarState
from iqdbc.car.honda.radar_interface import RadarInterface
from iqdbc.car.interfaces import CarInterfaceBase
from iqdbc.lvbs.car.honda.iq_values import HondaFlagsIQ, HondaSafetyFlagsIQ
TransmissionType = structs.CarParams.TransmissionType
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
DRIVABLE_GEARS = (structs.CarState.GearShifter.sport,)
@staticmethod
def get_pid_accel_limits(CP, CP_IQ, current_speed, cruise_speed):
if CP.carFingerprint in HONDA_BOSCH:
return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX
elif CP_IQ.enableGasInterceptor:
return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX
else:
# NIDECs don't allow acceleration near cruise_speed,
# so limit limits of pid to prevent windup
ACCEL_MAX_VALS = [CarControllerParams.NIDEC_ACCEL_MAX, 0.2]
ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2]
return CarControllerParams.NIDEC_ACCEL_MIN, np.interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "honda"
CAN = CanBus(ret, fingerprint)
if candidate in HONDA_BOSCH:
cfgs = [get_safety_config(structs.CarParams.SafetyModel.hondaBosch)]
if candidate in HONDA_BOSCH_CANFD and CAN.pt >= 4:
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
ret.safetyConfigs = cfgs
# The object scan survives openpilot longitudinal: the radar disable is subnet-scoped to the
# powertrain bus, while the scan rides the camera-side ACC-CAN
ret.radarUnavailable = docs or candidate not in HONDA_RADAR_SCAN_VERIFIED
# Disable the radar and let openpilot control longitudinal
# WARNING: THIS DISABLES AEB!
# If Bosch radarless, this blocks ACC messages from the camera
ret.alphaLongitudinalAvailable = True
ret.openpilotLongitudinalControl = alpha_long
ret.pcmCruise = not ret.openpilotLongitudinalControl
else:
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hondaNidec)]
ret.openpilotLongitudinalControl = True
ret.pcmCruise = True
if candidate == CAR.HONDA_CRV_5G:
ret.enableBsm = 0x12f8bfa7 in fingerprint[CAN.radar]
# Detect Bosch cars with new HUD msgs
if any(0x33DA in f for f in fingerprint.values()):
ret.flags |= HondaFlags.BOSCH_EXT_HUD.value
if 0x184 in fingerprint[CAN.pt]:
ret.flags |= HondaFlags.HYBRID.value
if ret.flags & HondaFlags.ALLOW_MANUAL_TRANS and all(msg not in fingerprint[CAN.pt] for msg in (0x191, 0x1A3)):
# Manual transmission support for allowlisted cars only, to prevent silent fall-through on auto-detection failures
ret.transmissionType = TransmissionType.manual
elif 0x191 in fingerprint[CAN.pt] and candidate != CAR.ACURA_RDX:
# Traditional CVTs, gearshift position in GEARBOX_CVT
ret.transmissionType = TransmissionType.cvt
else:
# Traditional autos, direct-drive EVs and eCVTs, gearshift position in GEARBOX_AUTO
ret.transmissionType = TransmissionType.automatic
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
ret.steerActuatorDelay = 0.1
if candidate in HONDA_BOSCH:
if candidate in HONDA_BOSCH_RADARLESS:
ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model
ret.longitudinalActuatorDelay = 0.25 # s
elif candidate in HONDA_BOSCH_CANFD:
ret.longitudinalActuatorDelay = 0.05 # near zero, canfd seems to have stock feedforward correction
else:
ret.longitudinalActuatorDelay = 0.25 # s, per Bosch A log
else:
# default longitudinal tuning for all hondas
ret.longitudinalTuning.kiBP = [0., 5., 35.]
ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5]
# Disable control if EPS mod detected
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
ret.dashcamOnly = True
if candidate == CAR.HONDA_CIVIC:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
elif candidate in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate == CAR.HONDA_CIVIC_2022:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 5120], [0, 5120]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[0, 10], [0.05, 0.5]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[0, 10], [0.0125, 0.125]]
elif candidate == CAR.HONDA_ACCORD:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
if ret.transmissionType == TransmissionType.manual:
CarControllerParams.BOSCH_GAS_LOOKUP_BP = [-0.2, 2.0]
elif candidate == CAR.HONDA_ACCORD_11G:
ret.steerActuatorDelay = 0.22
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 12747], [0, 12747]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.18]]
elif candidate == CAR.ACURA_ILX:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate in (CAR.HONDA_CRV, CAR.HONDA_CRV_EU):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.HONDA_CRV_5G:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.HONDA_CRV_HYBRID:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.HONDA_CRV_6G:
ret.steerActuatorDelay = 0.15
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 5100], [0, 5100]]
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.HONDA_FIT:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
elif candidate == CAR.HONDA_FREED:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
elif candidate in (CAR.HONDA_HRV, CAR.HONDA_HRV_3G):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
if candidate == CAR.HONDA_HRV:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
ret.wheelSpeedFactor = 1.025
else:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning
elif candidate == CAR.ACURA_RDX:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate == CAR.ACURA_RDX_3G:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4095], [0, 4095]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
elif candidate == CAR.HONDA_ODYSSEY:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
elif candidate == CAR.HONDA_ODYSSEY_TWN:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
elif candidate in (CAR.HONDA_PILOT, CAR.HONDA_PILOT_4G, CAR.HONDA_PASSPORT_4G, CAR.ACURA_MDX_4G_MMR):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[0, 10], [0.05, 0.5]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[0, 10], [0.0125, 0.125]]
elif candidate == CAR.HONDA_RIDGELINE:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
elif candidate in (CAR.HONDA_INSIGHT, CAR.HONDA_NBOX_2G):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
elif candidate == CAR.HONDA_E:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning
elif candidate == CAR.HONDA_ODYSSEY_5G_MMR:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3810], [0, 3810]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
ret.steerActuatorDelay = 0.15
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 2000]
if not ret.openpilotLongitudinalControl:
# When using stock ACC, the radar intercepts and filters steering commands the EPS would otherwise accept
ret.minSteerSpeed = 70. * CV.KPH_TO_MS
elif candidate == CAR.ACURA_TLX_2G_MMR:
ret.steerActuatorDelay = 0.15
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[0, 10], [0.05, 0.5]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[0, 10], [0.0125, 0.125]]
elif candidate == CAR.HONDA_CLARITY:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
else:
ret.steerActuatorDelay = 0.15
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate == CAR.HONDA_PILOT_4G:
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 2200]
elif candidate == CAR.ACURA_RDX_3G:
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 2200]
elif candidate == CAR.HONDA_CRV_6G and ret.flags & HondaFlags.HYBRID:
CarControllerParams.BOSCH_GAS_LOOKUP_BP = [-0.3, 2.0]
# These cars use alternate user brake msg (0x1BE)
if 0x1BE in fingerprint[CAN.pt] and candidate in HONDA_BOSCH:
ret.flags |= HondaFlags.BOSCH_ALT_BRAKE.value
if ret.flags & HondaFlags.BOSCH_ALT_BRAKE:
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.ALT_BRAKE.value
if candidate in HONDA_NIDEC_ALT_SCM_MESSAGES:
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.NIDEC_ALT.value
if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.BOSCH_LONG.value
if candidate in HONDA_BOSCH_RADARLESS:
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.RADARLESS.value
if candidate in HONDA_BOSCH_CANFD:
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.BOSCH_CANFD.value
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
# conflict with PCM acc
if (ret.transmissionType == TransmissionType.manual) and (not ret.openpilotLongitudinalControl):
ret.autoResumeSng = False
else:
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.HONDA_CIVIC})
if ret.autoResumeSng:
ret.minEnableSpeed = -1.
elif candidate == CAR.HONDA_ODYSSEY_TWN:
ret.minEnableSpeed = 19. * CV.MPH_TO_MS
else:
ret.minEnableSpeed = 25.51 * CV.MPH_TO_MS
ret.steerLimitTimer = 0.8
ret.radarDelay = 0.1
return ret
@staticmethod
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]],
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
CAN = CanBus(stock_cp, fingerprint)
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
ret.flags |= HondaFlagsIQ.EPS_MODIFIED.value
stock_cp.dashcamOnly = False
if bool(stock_cp.flags & HondaFlags.NIDEC) and bool(stock_cp.flags & HondaFlags.HYBRID):
ret.flags |= HondaFlagsIQ.NIDEC_HYBRID.value
ret.iqSafetyFlags |= HondaSafetyFlagsIQ.NIDEC_HYBRID
# some hybrids use a different brake hold
if 0x223 in fingerprint[CAN.pt]:
ret.flags |= HondaFlagsIQ.HYBRID_ALT_BRAKEHOLD.value
if 0x35E in fingerprint[CAN.pt]:
ret.flags |= HondaFlagsIQ.HAS_CAMERA_MESSAGES.value
if candidate == CAR.HONDA_CIVIC:
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
# stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
# stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680
# modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180
# stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108
# modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480
# note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]]
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.1]]
elif candidate in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL):
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]]
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.09]] # 2.5x Modded EPS
elif candidate == CAR.HONDA_CIVIC_2022:
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]]
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.09]] # 2.5x Modded EPS
elif candidate == CAR.HONDA_ACCORD:
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.09]]
elif candidate == CAR.HONDA_CRV_5G:
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
# stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F
# stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400
# modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.21], [0.07]]
elif candidate == CAR.HONDA_CLARITY:
stock_cp.autoResumeSng = True
stock_cp.minEnableSpeed = -1
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
for fw in car_fw:
if fw.ecu == "eps" and b"-" not in fw.fwVersion and b"," in fw.fwVersion:
stock_cp.lateralTuning.pid.kf = 0.00004
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 5760, 15360], [0, 2560, 3840]]
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.1575], [0.05175]]
elif fw.ecu == "eps" and b"-" in fw.fwVersion and b"," in fw.fwVersion:
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 5760, 10240], [0, 2560, 3840]]
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.1]]
else:
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2560], [0, 2560]]
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.8], [0.24]]
if candidate in HONDA_BOSCH:
pass
else:
ret.enableGasInterceptor = 0x201 in fingerprint[CAN.pt]
stock_cp.pcmCruise = not ret.enableGasInterceptor
if ret.enableGasInterceptor and candidate not in HONDA_BOSCH:
ret.iqSafetyFlags |= HondaSafetyFlagsIQ.GAS_INTERCEPTOR
stock_cp.autoResumeSng = stock_cp.autoResumeSng or ret.enableGasInterceptor
if candidate == CAR.HONDA_CITY_7G:
ret.longitudinalStoppingSpeedOverride = 2.0
ret.stoppingDecelRateOverride = 0.3
else:
ret.longitudinalStoppingSpeedOverride = 0.5
ret.stoppingDecelRateOverride = 0.1
return ret
@staticmethod
def init(CP, CP_IQ, can_recv, can_send, communication_control=None):
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl:
if communication_control is None and CP.carFingerprint in HONDA_BOSCH_CANFD:
# CAN FD: only clear DTCs here; the radar silencing itself is deferred to CarController until
# the comma relay is confirmed open. init() runs while the panda is still in the ELM327 safety
# mode, and silencing the radar from here raced the safety-mode switch: whenever the switch
# took longer than ~110 ms after radar silence, the brake module latched CRUISE_FAULT for the
# entire drive.
#
# The brake module's radar lost-communication DTC matures over trips (Honda two-trip
# detection): once confirmed from a previous drive, the very next comm-loss detection faults
# ~0.16 s after the radar goes silent. Broadcast-clear stored DTCs on the powertrain and
# camera buses every drive to reset the maturation counter, and clear the radar's own stored
# DTCs so codes accumulated while it was disabled don't re-fault a later drive. Clearing must
# precede the radar silence because a DTC clear can take an ECU several hundred ms.
# NOTE: ELM327 safety mode allows the 29-bit functional diagnostic address on every bus, so
# the broadcast needs no TX allowlist entry in the car safety mode
clear_all_dtcs(can_send, [CanBus(CP).pt, CanBus(CP).camera])
clear_ecu_dtcs(can_recv, can_send, bus=CanBus(CP).pt, addr=0x18DAB0F1)
else:
# 0x80 silences response
if communication_control is None:
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.DISABLE_RX_DISABLE_TX,
uds.MESSAGE_TYPE.NORMAL_AND_NETWORK_MANAGEMENT])
disable_ecu(can_recv, can_send, bus=CanBus(CP).pt, addr=0x18DAB0F1, com_cont_req=communication_control)
@staticmethod
def deinit(CP, can_recv, can_send):
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.ENABLE_RX_ENABLE_TX,
uds.MESSAGE_TYPE.NORMAL_AND_NETWORK_MANAGEMENT])
CarInterface.init(CP, None, can_recv, can_send, communication_control)

92
artifacts/package_runtime/iqdbc/car/honda/radar_interface.py Normal file
View File

@@ -0,0 +1,92 @@
#!/usr/bin/env python3
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.interfaces import RadarInterfaceBase
from iqdbc.car.honda.radar_scan import SCAN_DBC_NAME, HondaRadarScanner
from iqdbc.car.honda.values import DBC
def _create_nidec_can_parser(car_fingerprint):
radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
messages = [(m, 20) for m in radar_messages]
return CANParser(DBC[car_fingerprint][Bus.radar], messages, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP, CP_IQ):
super().__init__(CP, CP_IQ)
self.track_id = 0
self.radar_fault = False
self.radar_wrong_config = False
self.radar_off_can = CP.radarUnavailable
self.scanner = None
if self.radar_off_can:
self.rcp = None
self.trigger_msg = 0x445
elif DBC[CP.carFingerprint].get(Bus.radar) == SCAN_DBC_NAME:
self.scanner = HondaRadarScanner(CP)
self.rcp = self.scanner.rcp
self.pts = self.scanner.pts
self.trigger_msg = self.scanner.trigger_msg
else:
# Nidec
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
self.trigger_msg = 0x445
self.updated_messages = set()
def update(self, can_strings):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
if self.radar_off_can:
return super().update(None)
vls = self.rcp.update(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
if self.scanner is not None and self.scanner.sweep_overdue():
return self.scanner.quiet_bus_radardata()
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
def _update(self, updated_messages):
if self.scanner is not None:
return self.scanner.process_sweep(updated_messages)
ret = structs.RadarData()
for ii in sorted(updated_messages):
cpt = self.rcp.vl[ii]
if ii == 0x400:
# check for radar faults
self.radar_fault = cpt['RADAR_STATE'] != 0x79
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = structs.RadarData.RadarPoint()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
self.pts[ii].measured = True
else:
if ii in self.pts:
del self.pts[ii]
if not self.rcp.can_valid:
ret.errors.canError = True
if self.radar_fault:
ret.errors.radarFault = True
if self.radar_wrong_config:
ret.errors.wrongConfig = True
ret.points = list(self.pts.values())
return ret

396
artifacts/package_runtime/iqdbc/car/honda/radar_scan.py Normal file
View File

@@ -0,0 +1,396 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
from collections import deque
from dataclasses import dataclass, field
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.honda.hondacan import CanBus
from iqdbc.car.honda.values import DBC
from iqdbc.dbc.generator.honda.honda_radar_scan import QUARTET_KINDS, SCAN_SLOTS, frame_address
SCAN_DBC_NAME = 'honda_radar_scan_generated'
SWEEP_HZ = 15
SLOT_ADDRS = [tuple(frame_address(slot, kind) for kind in (*QUARTET_KINDS, "MOTION")) for slot in range(SCAN_SLOTS)]
ALL_SCAN_ADDRS = [addr for addrs in SLOT_ADDRS for addr in addrs]
# slot 15's IDENT frame closes every observed sweep's quartet family; its MOTION companion follows
# but must never gate an otherwise valid sweep, so the quartet frame stays the trigger
SWEEP_TRIGGER_ADDR = SLOT_ADDRS[SCAN_SLOTS - 1][3]
DIST_LSB_M = 0.05712
DIST_BIAS_M = -3.0
BEARING_LSB_RAD = 1.0 / 2048.0
BEARING_ZERO = 1024
STATE_INVALID = 0xF
DIST_RAW_INVALID = 0xFFF
BEARING_RAW_INVALID = 0x7FF
AGE_RAW_INVALID = 0xFFF
HANDLE_MIN = 1
HANDLE_MAX = 0x3F
CLOSING_SPEED_RAW_INVALID = 0x7FE
CLOSING_SPEED_RAW_MIN = 0
CLOSING_SPEED_RAW_MAX = 1728
CLOSING_SPEED_RAW_ZERO = 864
CLOSING_SPEED_LSB_MPS = 1.0 / 64.0
# replay-derived quality gate: the native speed field degrades gradually with its sigma companion;
# above this the field is no longer authoritative and the decoder coasts instead
CLOSING_SPEED_SIGMA_TRUST_MAX = 511
DIST_RATIO_RAW_INVALID = 0x3FF
DIST_RATIO_LSB = 0.001
DIST_RATIO_BIAS = 0.5
# replay-derived acceptance gates, not recovered firmware constants; innovation is measured from the
# previous ACCEPTED observation so a reset can never become the baseline for following sweeps
DIST_SIGMA_DEGRADED_RAW = 4
DIST_INNOVATION_SOFT_M = 2.0
DIST_INNOVATION_HARD_M = 5.0
RAW_RATE_LIMIT_MPS = 50.0
HISTORY_LEN = 8
QUIET_TIMEOUT_S = 0.20
def decode_closing_speed(raw, sigma_raw=None):
if raw is None:
return None
raw = int(raw)
if raw == CLOSING_SPEED_RAW_INVALID or not CLOSING_SPEED_RAW_MIN <= raw <= CLOSING_SPEED_RAW_MAX:
return None
if sigma_raw is not None and int(sigma_raw) > CLOSING_SPEED_SIGMA_TRUST_MAX:
return None
return (raw - CLOSING_SPEED_RAW_ZERO) * CLOSING_SPEED_LSB_MPS
def decode_dist_ratio(raw):
if raw is None:
return None
raw = int(raw)
if raw == DIST_RATIO_RAW_INVALID or not 0 <= raw < DIST_RATIO_RAW_INVALID:
return None
return DIST_RATIO_BIAS + DIST_RATIO_LSB * raw
def ratio_implied_rate(raw, dist, dt):
ratio = decode_dist_ratio(raw)
if ratio is None or dt <= 0.0 or not math.isfinite(dist):
return None
return dist * (1.0 - ratio) / dt
def reading_degraded(dist_sigma_raw, presence_raw, speed_sigma_raw):
geometry_bad = dist_sigma_raw >= DIST_SIGMA_DEGRADED_RAW or presence_raw in (0, 0x7F)
motion_bad = speed_sigma_raw is not None and speed_sigma_raw > CLOSING_SPEED_SIGMA_TRUST_MAX
return geometry_bad or motion_bad
@dataclass
class SlotReading:
slot: int
cycle: int
age: int
handle: int
handle_ok: bool
coherent: bool
dist_raw: int = 0
bearing_raw: int = 0
dist_sigma_raw: int = 0
presence_raw: int = 0
speed_raw: int | None = None
speed_sigma_raw: int | None = None
ratio_raw: int | None = None
@dataclass
class ObjectLedger:
handle: int
prev_cycle: int | None = None
prev_age: int | None = None
last_seen_nanos: int | None = None
wire_slot: int | None = None
history: deque = field(default_factory=lambda: deque(maxlen=HISTORY_LEN))
held_speed: float | None = None
held_speed_nanos: int | None = None
def continues_incarnation(self, cycle: int, age: int) -> bool:
if self.prev_cycle is None or self.prev_age is None:
return False
cycle_delta = (cycle - self.prev_cycle) & 0xF
age_delta = (age - self.prev_age) & 0xFFF
return age_delta == 2 * cycle_delta
def restart_incarnation(self):
self.history.clear()
self.held_speed = None
self.held_speed_nanos = None
def held_speed_fresh(self, now: int) -> bool:
return (self.held_speed is not None and self.held_speed_nanos is not None and
(now - self.held_speed_nanos) * 1e-9 <= QUIET_TIMEOUT_S)
def create_scan_parser(CP) -> CANParser:
# the object scan is physically on the camera-side ACC-CAN
return CANParser(DBC[CP.carFingerprint][Bus.radar], [(addr, SWEEP_HZ) for addr in ALL_SCAN_ADDRS], CanBus(CP).camera)
class HondaRadarScanner:
def __init__(self, CP):
self.rcp = create_scan_parser(CP)
self.trigger_msg = SWEEP_TRIGGER_ADDR
self.pts: dict[int, structs.RadarData.RadarPoint] = {}
self._ledgers: dict[int, ObjectLedger] = {}
self._slot_handles: list[int | None] = [None] * SCAN_SLOTS
self._last_sweep_nanos = -1
def sweep_overdue(self) -> bool:
if self._last_sweep_nanos < 0:
return False
return (self.rcp._last_update_nanos - self._last_sweep_nanos) * 1e-9 > QUIET_TIMEOUT_S
def quiet_bus_radardata(self) -> structs.RadarData:
# whole-bus silence: drop everything and emit an EMPTY RadarData (not None) so radard sheds any
# lead within a cycle instead of freezing a phantom
self.pts.clear()
self._ledgers.clear()
self._slot_handles = [None] * SCAN_SLOTS
self._last_sweep_nanos = -1
ret = structs.RadarData()
if not self.rcp.can_valid:
ret.errors.canError = True
ret.errors.radarUnavailableTemporary = True
return ret
def _drop_ledger(self, handle: int):
self._ledgers.pop(handle, None)
self.pts.pop(handle, None)
for slot, bound in enumerate(self._slot_handles):
if bound == handle:
self._slot_handles[slot] = None
def _drop_expired_ledgers(self, now: int):
for handle, ledger in list(self._ledgers.items()):
if ledger.last_seen_nanos is not None and (now - ledger.last_seen_nanos) * 1e-9 > QUIET_TIMEOUT_S:
self._drop_ledger(handle)
def _read_slot(self, slot: int, updated_messages) -> SlotReading | None:
pos, shape, life, ident, motion = SLOT_ADDRS[slot]
if not all(addr in updated_messages for addr in (pos, shape, life, ident)):
# a missing CAN frame is not a lifecycle event; ledgers expire on their own staleness only
return None
v_pos, v_shape, v_life, v_ident = (self.rcp.vl[a] for a in (pos, shape, life, ident))
cycle = int(v_pos['CYCLE'])
if not (cycle == int(v_shape['CYCLE']) == int(v_life['CYCLE']) == int(v_ident['CYCLE'])):
# the quartet doesn't share one radar cycle: not a coherent observation this window
return None
state = int(v_pos['SCAN_STATE'])
dist_raw = int(v_pos['DIST_RAW'])
bearing_raw = int(v_pos['BEARING_RAW'])
age = int(v_life['AGE_RAW'])
handle = int(v_ident['OBJECT_HANDLE'])
reading = SlotReading(
slot=slot,
cycle=cycle,
age=age,
handle=handle,
handle_ok=HANDLE_MIN <= handle <= HANDLE_MAX,
coherent=(state != STATE_INVALID and dist_raw != DIST_RAW_INVALID and
bearing_raw != BEARING_RAW_INVALID and age != AGE_RAW_INVALID),
dist_raw=dist_raw,
bearing_raw=bearing_raw,
dist_sigma_raw=int(v_pos['DIST_SIGMA_RAW']),
presence_raw=int(v_shape['PRESENCE_RAW']),
)
# the MOTION companion only contributes when it rides the same cycle; its absence never
# invalidates the quartet, it only removes independent motion evidence
if motion in updated_messages:
v_motion = self.rcp.vl[motion]
if int(v_motion['CYCLE']) == cycle:
reading.speed_raw = int(v_motion['CLOSING_SPEED_RAW'])
reading.speed_sigma_raw = int(v_motion['CLOSING_SPEED_SIGMA_RAW'])
reading.ratio_raw = int(v_motion['DIST_RATIO_RAW'])
return reading
def _elect_by_handle(self, readings: list[SlotReading]) -> dict[int, SlotReading]:
# one CAN identity can never yield two points; ties prefer the wire slot already bound to the
# ledger, then the lower slot for deterministic handling of a malformed duplicate
elected: dict[int, SlotReading] = {}
for reading in readings:
if not (reading.coherent and reading.handle_ok):
# an invalid observation ends publication for the slot's current occupant without destroying
# persistent state; the object may be multiplexed elsewhere or return before its deadline
hidden = {self._slot_handles[reading.slot]}
if reading.handle_ok:
hidden.add(reading.handle)
for handle in hidden - {None}:
self.pts.pop(handle, None)
continue
current = elected.get(reading.handle)
if current is None:
elected[reading.handle] = reading
continue
bound_slot = self._ledgers[reading.handle].wire_slot if reading.handle in self._ledgers else None
current_rank = (0 if bound_slot == current.slot else 1, current.slot)
candidate_rank = (0 if bound_slot == reading.slot else 1, reading.slot)
if candidate_rank < current_rank:
elected[reading.handle] = reading
return elected
def _bind_slot(self, ledger: ObjectLedger, reading: SlotReading, now: int):
ledger.prev_cycle = reading.cycle
ledger.prev_age = reading.age
ledger.last_seen_nanos = now
ledger.wire_slot = reading.slot
for slot, bound in enumerate(self._slot_handles):
if slot != reading.slot and bound == ledger.handle:
self._slot_handles[slot] = None
self._slot_handles[reading.slot] = ledger.handle
def _coast_point(self, ledger: ObjectLedger, now: int, dist: float, y_rel: float):
# coasting keeps trustworthy geometry visible with the last authoritative motion, unmeasured,
# instead of publishing a synthesized rate; without fresh held motion the point drops
if ledger.held_speed_fresh(now):
point = self.pts.get(ledger.handle)
if point is not None:
point.dRel = dist
point.yRel = y_rel
point.vRel = ledger.held_speed
point.measured = False
return
ledger.held_speed = None
ledger.held_speed_nanos = None
self.pts.pop(ledger.handle, None)
def process_sweep(self, updated_messages) -> structs.RadarData:
ret = structs.RadarData()
if not self.rcp.can_valid:
ret.errors.canError = True
now = self.rcp._last_update_nanos
self._last_sweep_nanos = now
self._drop_expired_ledgers(now)
readings = [r for r in (self._read_slot(slot, updated_messages) for slot in range(SCAN_SLOTS)) if r is not None]
elected = self._elect_by_handle(readings)
elected_handles = set(elected)
for handle, reading in sorted(elected.items(), key=lambda item: item[1].slot):
# a wire-slot replacement ends publication for the old occupant, but not its persistent state
old_handle = self._slot_handles[reading.slot]
if old_handle is not None and old_handle != handle and old_handle not in elected_handles:
self.pts.pop(old_handle, None)
ledger = self._ledgers.get(handle)
if ledger is None:
ledger = ObjectLedger(handle=handle)
self._ledgers[handle] = ledger
if not ledger.continues_incarnation(reading.cycle, reading.age):
# the CAN identity stays the external key, but a lifecycle discontinuity starts a new
# incarnation and must not inherit the previous object's range-rate history
ledger.restart_incarnation()
self.pts.pop(handle, None)
dist = DIST_LSB_M * reading.dist_raw + DIST_BIAS_M
bearing = BEARING_LSB_RAD * (reading.bearing_raw - BEARING_ZERO)
# the radar consumes range as a forward-axis quantity; positive bearing is left of center,
# matching the RadarPoint.yRel sign contract
y_rel = dist * math.tan(bearing)
now_s = now * 1e-9
native_speed = decode_closing_speed(reading.speed_raw, reading.speed_sigma_raw)
unqualified_speed = decode_closing_speed(reading.speed_raw)
# a live native speed rejected only by its sigma companion: geometry acceptance is still decided
# on the same terms as every other sweep (high sigma correlates with bad/discontinuous range),
# and only then does the point coast instead of publishing a synthesized rate
sigma_veto = (native_speed is None and unqualified_speed is not None and
reading.speed_sigma_raw is not None and
reading.speed_sigma_raw > CLOSING_SPEED_SIGMA_TRUST_MAX)
degraded = reading_degraded(reading.dist_sigma_raw, reading.presence_raw, reading.speed_sigma_raw)
previous = ledger.history[-1] if ledger.history else None
ratio_rate = None
dist_rejected = False
if previous is not None:
prev_time, prev_dist = previous
dt = now_s - prev_time
if dt <= 0.0:
dist_rejected = True
else:
ratio = decode_dist_ratio(reading.ratio_raw)
ratio_rate = ratio_implied_rate(reading.ratio_raw, dist, dt)
residuals_m = []
if native_speed is not None:
residuals_m.append(abs(dist - (prev_dist + native_speed * dt)))
if ratio is not None:
residuals_m.append(abs(prev_dist - dist * ratio))
if residuals_m:
innovation_m = min(residuals_m)
dist_rejected = (innovation_m > DIST_INNOVATION_HARD_M or
(degraded and innovation_m > DIST_INNOVATION_SOFT_M))
else:
dist_rejected = abs((dist - prev_dist) / dt) > RAW_RATE_LIMIT_MPS
if dist_rejected:
# keep the last accepted point briefly as an unmeasured coast; rejected geometry is never
# published and never becomes the baseline for a later derivative
accepted_fresh = previous is not None and now_s - previous[0] <= QUIET_TIMEOUT_S
point = self.pts.get(handle)
if accepted_fresh and point is not None:
point.measured = False
else:
self.pts.pop(handle, None)
self._bind_slot(ledger, reading, now)
continue
if sigma_veto:
self._coast_point(ledger, now, dist, y_rel)
self._bind_slot(ledger, reading, now)
continue
# with neither a qualified native speed nor a usable ratio, the only remaining source is the
# raw one-sweep derivative, which must never become an authoritative measurement: coast instead.
# A true birth (no previous accepted sample) cannot mature this cycle regardless, so only
# intercept once a derivative would have something to poison
if native_speed is None and (ratio_rate is None or degraded) and previous is not None:
self._coast_point(ledger, now, dist, y_rel)
self._bind_slot(ledger, reading, now)
continue
ledger.history.append((now_s, dist))
speed = native_speed if native_speed is not None else ratio_rate
ledger.held_speed = speed
ledger.held_speed_nanos = now
# a birth observation has no range rate yet: keep it as history, publish only once a second
# coherent observation of the same identity supplies a finite rate
matured = len(ledger.history) >= 2 and math.isfinite(speed)
if matured:
if handle not in self.pts:
point = structs.RadarData.RadarPoint()
point.trackId = handle
point.aRel = float('nan')
point.yvRel = float('nan')
self.pts[handle] = point
self.pts[handle].dRel = dist
self.pts[handle].yRel = y_rel
self.pts[handle].vRel = speed
self.pts[handle].measured = True
else:
self.pts.pop(handle, None)
self._bind_slot(ledger, reading, now)
ret.points = [self.pts[handle] for handle in sorted(self.pts)]
return ret

0
artifacts/package_runtime/iqdbc/car/honda/tests/__init__.py Normal file
View File

166
artifacts/package_runtime/iqdbc/car/honda/tests/test_canfd_carcontroller.py Normal file
View File

@@ -0,0 +1,166 @@
from iqdbc.car import DT_CTRL, gen_empty_fingerprint, structs
from iqdbc.car.honda.interface import CarInterface
from iqdbc.car.honda.values import CAR
CANFD_CAR = CAR.HONDA_CRV_6G
RADAR_DIAG_ADDR = 0x18DAB0F1
ACC_CONTROL_ADDR = 0x1DF
ACC_HUD_ADDR = 0x30C
SCM_BUTTONS_ADDR = 0x296
RADAR_HUD_ADDR = 0x310
LANE_PATH_ADDR = 0x6CD5558
HUD_OBJECTS_ADDR = 0x6CD5559
RADAR_LEAD_ADDR = 0xF31AA5C
RADAR_LEAD2_ADDR = 0xF31AA52
SUPPLEMENTAL_ADDR = 0x1A45AA4E
LOOKALIKE_ADDRS = (RADAR_HUD_ADDR, LANE_PATH_ADDR, HUD_OBJECTS_ADDR, RADAR_LEAD_ADDR, RADAR_LEAD2_ADDR, SUPPLEMENTAL_ADDR)
EXT_DIAG_SESSION = b'\x02\x10\x03\x00\x00\x00\x00\x00'
COMM_CONTROL_DISABLE = b'\x03\x28\x83\x03\x00\x00\x00\x00'
def build_long_interface():
fingerprint = gen_empty_fingerprint()
CP = CarInterface.get_params(CANFD_CAR, fingerprint, [], False, False, False)
CP.openpilotLongitudinalControl = True
CP.pcmCruise = False
CP_IQ = CarInterface.get_params_iq(CP, CANFD_CAR, fingerprint, [], False, False, False)
return CarInterface(CP, CP_IQ)
def make_cc(enabled=True):
CC = structs.CarControl()
CC.enabled = enabled
CC.latActive = enabled
CC.longActive = enabled
return CC.as_reader()
class CanfdControllerHarness:
def __init__(self):
self.ci = build_long_interface()
self.cs = self.ci.CS
self.ci.update([])
self.now_nanos = 0
self.set_radar(alive=True, relay_open=False)
self.set_ticks()
def set_radar(self, alive, relay_open):
self.cs.stock_acc_alive = alive
self.cs.canfd_relay_open = relay_open
def set_ticks(self, hud=False, supp=False, five=False, fifty=False):
self.cs.hud_tick = hud
self.cs.supp_tick = supp
self.cs.radar_5hz_tick = five
self.cs.radar_50hz_tick = fifty
def step(self, CC=None, model=None):
self.now_nanos += int(DT_CTRL * 1e9)
_, can_sends = self.ci.apply(CC or make_cc(), structs.IQCarControl(), self.now_nanos, model)
return can_sends
@staticmethod
def by_addr(can_sends, addr):
return [m for m in can_sends if m[0] == addr]
class TestCanfdDeferredRadarDisable:
def setup_method(self):
self.h = CanfdControllerHarness()
def test_no_disable_requests_before_relay_open(self):
for _ in range(20):
sends = self.h.step()
assert not self.h.by_addr(sends, RADAR_DIAG_ADDR)
assert not self.h.by_addr(sends, ACC_CONTROL_ADDR)
assert not any(self.h.by_addr(sends, a) for a in LOOKALIKE_ADDRS)
def test_disable_handshake_after_relay_open(self):
self.h.set_radar(alive=True, relay_open=True)
payloads = []
for _ in range(101):
for msg in self.h.by_addr(self.h.step(), RADAR_DIAG_ADDR):
payloads.append(msg[1])
assert payloads == [EXT_DIAG_SESSION, COMM_CONTROL_DISABLE, EXT_DIAG_SESSION, COMM_CONTROL_DISABLE, EXT_DIAG_SESSION]
def test_tester_present_keeps_radar_down_once_silent(self):
self.h.set_radar(alive=False, relay_open=True)
payloads = []
for _ in range(60):
payloads += [m[1] for m in self.h.by_addr(self.h.step(), RADAR_DIAG_ADDR)]
assert payloads == [b'\x02\x3E\x80\x00\x00\x00\x00\x00'] * 6
class TestCanfdReplacementStream:
def setup_method(self):
self.h = CanfdControllerHarness()
self.h.set_radar(alive=False, relay_open=True)
def test_acc_control_every_second_frame(self):
seen = [bool(self.h.by_addr(self.h.step(), ACC_CONTROL_ADDR)) for _ in range(10)]
assert sum(seen) == 5
def test_no_acc_control_while_stock_alive(self):
self.h.set_radar(alive=True, relay_open=True)
for _ in range(10):
assert not self.h.by_addr(self.h.step(), ACC_CONTROL_ADDR)
def test_lookalikes_mirrored_byte_identical_on_both_buses(self):
self.h.set_ticks(hud=True, supp=True, five=True, fifty=True)
sends = self.h.step()
for addr in LOOKALIKE_ADDRS:
msgs = self.h.by_addr(sends, addr)
assert len(msgs) == 2, hex(addr)
buses = sorted(m[2] for m in msgs)
assert buses == [0, 2], hex(addr)
assert msgs[0][1] == msgs[1][1], hex(addr)
def test_no_lookalikes_without_ticks(self):
sends = self.h.step()
for addr in (RADAR_HUD_ADDR, RADAR_LEAD_ADDR, RADAR_LEAD2_ADDR, SUPPLEMENTAL_ADDR, LANE_PATH_ADDR, HUD_OBJECTS_ADDR):
assert not self.h.by_addr(sends, addr)
def test_mux_sweep_contiguous_across_banks(self):
self.h.set_ticks(fifty=True)
muxes = []
for _ in range(45):
msgs = self.h.by_addr(self.h.step(), LANE_PATH_ADDR)
muxes.append(msgs[0][1][0] >> 2)
sweep = list(range(1, 11)) + list(range(17, 27)) + list(range(33, 43)) + list(range(49, 59))
assert muxes == (sweep + sweep)[:45]
def test_acc_hud_rides_hud_tick(self):
assert not self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
self.h.set_ticks(hud=True)
assert self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
self.h.set_ticks()
assert not self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
class TestCanfdButtonTakeover:
def setup_method(self):
self.h = CanfdControllerHarness()
self.h.set_radar(alive=False, relay_open=True)
def test_buttons_streamed_to_camera_while_engaged(self):
seen = 0
for _ in range(20):
for msg in self.h.by_addr(self.h.step(), SCM_BUTTONS_ADDR):
assert msg[2] == 2
seen += 1
assert seen == 5
def test_no_button_stream_when_disengaged(self):
for _ in range(20):
assert not self.h.by_addr(self.h.step(make_cc(enabled=False)), SCM_BUTTONS_ADDR)
def test_ambient_light_echoed(self):
self.h.cs.scm_ambient_light = 0x77
for _ in range(4):
msgs = self.h.by_addr(self.h.step(), SCM_BUTTONS_ADDR)
if msgs:
assert msgs[0][1][2] == 0x77
return
raise AssertionError("no SCM_BUTTONS takeover frame seen")

202
artifacts/package_runtime/iqdbc/car/honda/tests/test_canfd_radar_state.py Normal file
View File

@@ -0,0 +1,202 @@
import pytest
from iqdbc.can import CANPacker
from iqdbc.car import Bus, DT_CTRL, gen_empty_fingerprint
from iqdbc.car.honda.interface import CarInterface
from iqdbc.car.honda.values import CAR, DBC
from iqdbc.car.common.conversions import Conversions as CV
CANFD_CAR = CAR.HONDA_CRV_6G
RADARLESS_CAR = CAR.HONDA_CIVIC_2022
CAMERA_MESSAGES_ADDR = 0x35E
def build_car(candidate, extra_pt_addrs=()):
fingerprint = gen_empty_fingerprint()
for addr in extra_pt_addrs:
fingerprint[0][addr] = 8
CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False)
CP_IQ = CarInterface.get_params_iq(CP, candidate, fingerprint, [], False, False, False)
return CarInterface(CP, CP_IQ)
class CanFeed:
def __init__(self, ci, dbc_name):
self.ci = ci
self.packer = CANPacker(dbc_name)
self.nanos = 0
# the first CarState.update lazily subscribes vl-read messages, so run one empty
# cycle before feeding data or the first fed frame of those messages is dropped
self.step()
self.ci.CS.update(self.ci.can_parsers)
def step(self, msgs=()):
self.nanos += int(DT_CTRL * 1e9)
packed = [self.packer.make_can_msg(name, bus, values) for name, bus, values in msgs]
for parser in self.ci.can_parsers.values():
parser.update([self.nanos, packed])
class TestHondaCanfdRadarState:
def setup_method(self):
self.ci = build_car(CANFD_CAR)
self.cs = self.ci.CS
self.feed = CanFeed(self.ci, DBC[CANFD_CAR][Bus.pt])
def update(self, msgs=()):
self.feed.step(msgs)
return self.cs.update(self.ci.can_parsers)
def test_parsers_include_radar_bus(self):
assert Bus.radar in self.ci.can_parsers
assert self.ci.can_parsers[Bus.radar].bus == 1
def test_50hz_tick_fires_one_frame_before_next_tick(self):
ticks = []
for frame in range(20):
msgs = [("RADAR_50HZ_TICK_REFERENCE", 1, {})] if frame % 2 == 0 else []
self.update(msgs)
ticks.append(self.cs.radar_50hz_tick)
assert ticks[2:] == [frame % 2 == 1 for frame in range(2, 20)]
def test_hud_tick_fires_one_frame_before_next_tick(self):
fired = []
for frame in range(40):
msgs = [("RADAR_HUD_TICK_REFERENCE", 1, {})] if frame % 10 == 0 else []
self.update(msgs)
if self.cs.hud_tick:
fired.append(frame)
assert fired == [9, 19, 29, 39]
def test_5hz_tick_fires_at_stock_radar_lead_offset(self):
fired = []
for frame in range(60):
msgs = [("RADAR_REFERENCE", 0, {})] if frame % 20 == 0 else []
self.update(msgs)
if self.cs.radar_5hz_tick:
fired.append(frame)
assert fired == [11, 31, 51]
def test_stock_acc_alive_until_four_silent_frames(self):
for frame in range(11):
msgs = [("ACC_CONTROL", 0, {})] if frame % 2 == 0 else []
self.update(msgs)
assert self.cs.stock_acc_alive
silent_state = []
for _ in range(6):
self.update()
silent_state.append(self.cs.stock_acc_alive)
assert silent_state == [True, True, True, False, False, False]
self.update([("ACC_CONTROL", 0, {})])
assert self.cs.stock_acc_alive
def test_relay_open_when_camera_steering_disappears(self):
for _ in range(10):
self.update([("STEERING_CONTROL", 0, {})])
assert not self.cs.canfd_relay_open
assert self.cs.camera_steer_seen
open_state = []
for _ in range(7):
self.update()
open_state.append(self.cs.canfd_relay_open)
assert open_state == [False, False, False, False, True, True, True]
def test_relay_open_fallback_without_camera(self):
primed_frames = self.cs.canfd_frames
for frame in range(510):
self.update()
assert self.cs.canfd_relay_open == (primed_frames + frame + 1 >= 500)
def test_ambient_light_echoed_from_scm_buttons(self):
self.update([("SCM_BUTTONS", 0, {"AMBIENT_LIGHT_MAYBE": 0x5A})])
assert self.cs.scm_ambient_light == 0x5A
class TestHondaNonCanfdRadarState:
def test_no_radar_parser_and_ticks_stay_low(self):
ci = build_car(RADARLESS_CAR)
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
assert Bus.radar not in ci.can_parsers
for _ in range(5):
feed.step()
ci.CS.update(ci.can_parsers)
assert not ci.CS.radar_50hz_tick
assert not ci.CS.hud_tick
assert not ci.CS.supp_tick
assert not ci.CS.radar_5hz_tick
class TestCanfdLongInterface:
def test_alpha_long_available_on_canfd(self):
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], False, False, False)
assert CP.alphaLongitudinalAvailable
assert not CP.openpilotLongitudinalControl
assert CP.pcmCruise
def test_alpha_long_enabled_on_canfd(self):
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
assert CP.openpilotLongitudinalControl
assert not CP.pcmCruise
assert CP.longitudinalActuatorDelay == pytest.approx(0.05)
def test_canfd_long_init_clears_dtcs_without_disabling_radar(self, mocker):
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
clear_ecu = mocker.patch("iqdbc.car.honda.interface.clear_ecu_dtcs")
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
CarInterface.init(CP, None, None, None)
assert clear_all.call_count == 1
assert clear_all.call_args.args[1] == [0, 2]
assert clear_ecu.call_count == 1
assert disable.call_count == 0
def test_canfd_deinit_reenables_radar(self, mocker):
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
CarInterface.deinit(CP, None, None)
assert clear_all.call_count == 0
assert disable.call_count == 1
def test_bosch_a_long_init_still_disables_radar(self, mocker):
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
CP = CarInterface.get_params(CAR.HONDA_ACCORD, gen_empty_fingerprint(), [], True, False, False)
CarInterface.init(CP, None, None, None)
assert clear_all.call_count == 0
assert disable.call_count == 1
class TestHondaDashboardSpeedLimit:
def build(self, candidate, with_camera_messages):
extra = (CAMERA_MESSAGES_ADDR,) if with_camera_messages else ()
return build_car(candidate, extra_pt_addrs=extra)
@pytest.mark.parametrize("sign_value,expected_mph", [(101, 25), (97, 5), (113, 85)])
def test_speed_limit_sign_reported(self, sign_value, expected_mph):
ci = self.build(RADARLESS_CAR, True)
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": sign_value})])
_, ret_iq = ci.CS.update(ci.can_parsers)
assert ret_iq.speedLimit == pytest.approx(expected_mph * CV.MPH_TO_MS)
@pytest.mark.parametrize("sign_value", [125, 0, 32])
def test_invalid_sign_reports_no_limit(self, sign_value):
ci = self.build(RADARLESS_CAR, True)
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": sign_value})])
_, ret_iq = ci.CS.update(ci.can_parsers)
assert ret_iq.speedLimit == 0.0
def test_without_camera_messages_flag_no_limit(self):
ci = self.build(RADARLESS_CAR, False)
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": 101})])
_, ret_iq = ci.CS.update(ci.can_parsers)
assert ret_iq.speedLimit == 0.0

235
artifacts/package_runtime/iqdbc/car/honda/tests/test_dash_rendering.py Normal file
View File

@@ -0,0 +1,235 @@
import math
from types import SimpleNamespace
import numpy as np
from iqdbc.can import CANPacker
from iqdbc.car.honda import dash_lane, dash_objects
V_EGO = 30.0
def model_at(center_y):
x = list(np.linspace(0.0, 110.0, 23))
def line(y):
return SimpleNamespace(x=x, y=[y] * len(x))
return SimpleNamespace(laneLines=[line(center_y + 3.3), line(center_y + 1.65), line(center_y - 1.65), line(center_y - 3.3)],
laneLineProbs=[0.0, 1.0, 1.0, 0.0],
leadsV3=[])
def lane_xy(center_y):
m = model_at(center_y)
return m.laneLines[1].x, [(a + b) / 2.0 for a, b in zip(m.laneLines[1].y, m.laneLines[2].y, strict=True)]
class TestLanePathSlew:
def test_first_fit_shown_unslewed(self):
renderer = dash_lane.LanePathRenderer()
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
def test_step_is_rate_limited(self):
renderer = dash_lane.LanePathRenderer()
prev = renderer.update(model_at(0.0), V_EGO, 0.0).offsets
assert all(o == 0 for o in prev)
target = dash_lane.encode_lane_path(*lane_xy(-2.0))
max_step = math.ceil(dash_lane.SLEW_MAX_STEP)
for _ in range(10):
cur = renderer.update(model_at(-2.0), V_EGO, 0.0).offsets
for p, c, t in zip(prev, cur, target, strict=True):
assert abs(c - p) <= max_step
assert abs(t - c) <= abs(t - p)
prev = cur
assert prev == target
def test_full_scale_takes_two_seconds(self):
renderer = dash_lane.LanePathRenderer()
renderer.update(model_at(0.0), V_EGO, 0.0)
target = dash_lane.encode_lane_path(*lane_xy(-100.0))
assert all(t == dash_lane.OFFSET_VALID_MAX for t in target)
n_updates = round(dash_lane.SLEW_FULL_SCALE_S * dash_lane.SLEW_RATE_HZ)
for i in range(n_updates):
lane = renderer.update(model_at(-100.0), V_EGO, 0.0)
if i < n_updates - 1:
assert lane.offsets != target
assert lane.offsets == target
def test_blank_resets_slew(self):
renderer = dash_lane.LanePathRenderer()
renderer.update(model_at(0.0), V_EGO, 0.0)
lane = renderer.update(None, V_EGO, 0.0)
assert lane.offsets == [dash_lane.OFFSET_UNAVAILABLE] * dash_lane.POINT_COUNT
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
def test_short_path_passthrough_and_reset(self):
renderer = dash_lane.LanePathRenderer()
renderer.update(model_at(0.0), V_EGO, 0.0)
short = model_at(-2.0)
for ll in short.laneLines:
ll.x = ll.x[:10]
ll.y = ll.y[:10]
lane = renderer.update(short, V_EGO, 0.0)
assert lane.offsets == [dash_lane.OFFSET_UNAVAILABLE] * dash_lane.POINT_COUNT
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
class TestLaneLineHysteresis:
def test_single_line_offset_and_hysteresis(self):
renderer = dash_lane.LanePathRenderer()
m = model_at(0.0)
m.laneLineProbs = [0.0, 0.0, 1.0, 0.0]
lane = renderer.update(m, V_EGO, 0.0)
assert not lane.left_line and lane.right_line
assert lane.offsets == dash_lane.encode_lane_path(m.laneLines[2].x, [y - dash_lane.HALF_LANE_M for y in m.laneLines[2].y])
# a left prob between OFF and ON must not switch the left line on
m.laneLineProbs = [0.0, (dash_lane.LINE_PROB_OFF + dash_lane.LINE_PROB_ON) / 2, 1.0, 0.0]
lane = renderer.update(m, V_EGO, 0.0)
assert not lane.left_line
# once on, the same mid prob keeps it on
m.laneLineProbs = [0.0, dash_lane.LINE_PROB_ON, 1.0, 0.0]
assert renderer.update(m, V_EGO, 0.0).left_line
m.laneLineProbs = [0.0, (dash_lane.LINE_PROB_OFF + dash_lane.LINE_PROB_ON) / 2, 1.0, 0.0]
assert renderer.update(m, V_EGO, 0.0).left_line
class TestCanfdReshape:
def test_idle_pattern_when_blank(self):
assert dash_lane.canfd_lane_offsets(dash_lane.RenderedLane()) == dash_lane.CANFD_IDLE_OFFSETS
assert dash_lane.canfd_lane_length(dash_lane.RenderedLane()) == dash_lane.CANFD_MIN_VALID_PTS
def test_terminated_prefix_matches_length_law(self):
for v_ego, expected in ((0.0, 7), (10.0, 15), (19.0, 23), (38.0, 23)):
lane = dash_lane.RenderedLane(offsets=[5] * dash_lane.POINT_COUNT, reach=1.0, v_ego=v_ego)
n = dash_lane.canfd_lane_length(lane)
assert n == expected
offs = dash_lane.canfd_lane_offsets(lane)
assert offs[:n] == [5] * n
assert offs[n:] == [dash_lane.OFFSET_UNAVAILABLE] * (dash_lane.POINT_COUNT - n)
class TestMuxMapping:
def test_mux_cycle_covers_all_banks(self):
assert len(dash_lane.MUX_CYCLE) == 40
assert set(dash_lane.MUX_CYCLE) == set(range(1, 11)) | set(range(17, 27)) | set(range(33, 43)) | set(range(49, 59))
def test_lane_path_frame_selects_offsets_by_mux(self):
packer = CANPacker("honda_bosch_radarless_generated")
offsets = list(range(40))
for mux in dash_lane.MUX_CYCLE:
addr, dat, bus = dash_lane.create_lane_path(packer, 0, offsets, mux)
base = ((mux - 1) % 16) * 4
raw_mux = dat[0] >> 2
assert raw_mux == mux
assert base < 40
class TestDashObjectAuthor:
def make_lead(self, prob=0.9, d=30.0, y=0.0, v=0.0):
status = prob >= dash_objects.LEAD_PROB_ON
return dash_objects.ModelLead(status, d, y, v, prob=prob)
def payload(self, msg):
return msg[1]
def test_inactive_slot_bytes_match_stock_sentinel(self):
packer = CANPacker("honda_common_canfd_generated")
author = dash_objects.DashObjectAuthor()
msg = author.create(packer, 0, self.make_lead(prob=0.0), None, 2, 0.0)
parsed_long = ((self.payload(msg)[4] << 2) | (self.payload(msg)[5] >> 6)) & 0x3FF
assert parsed_long == 1023
def test_lead_rendered_in_slot0_only(self):
packer = CANPacker("honda_common_canfd_generated")
author = dash_objects.DashObjectAuthor()
lead = self.make_lead()
slot0 = author.create(packer, 0, lead, None, 1, 0.0)
slot3 = author.create(packer, 0, lead, None, 4, 0.02)
assert self.payload(slot0)[1] != 0
assert self.payload(slot3)[1] & 0xF8 == 0
def test_lead_prob_hysteresis_and_hold(self):
packer = CANPacker("honda_common_canfd_generated")
author = dash_objects.DashObjectAuthor()
now = 0.0
def object_id(prob):
nonlocal now
now += 0.02
msg = author.create(packer, 0, self.make_lead(prob=prob), None, 1, now)
return self.payload(msg)[1] >> 3
assert object_id(0.6) != 0
# dips below ON but above OFF keep rendering
assert object_id(0.4) != 0
# a full drop is bridged for LEAD_HOLD_S
assert object_id(0.0) != 0
now += dash_objects.LEAD_HOLD_S
assert object_id(0.0) == 0
def test_reid_on_range_discontinuity(self):
ident = dash_objects.LeadIdentity()
now = 0.0
first = ident.update(True, 30.0, 0.0, now)
# stay steady past the re-id refractory window
for _ in range(int(dash_objects.REID_REFRACTORY / 0.02) + 10):
now += 0.02
same = ident.update(True, 30.0, 0.0, now)
assert same == first
now += 0.02
assert ident.update(True, 60.0, 0.0, now) != first
def test_camera_lead_never_forwarded(self):
packer = CANPacker("honda_bosch_radarless_generated")
author = dash_objects.DashObjectAuthor()
tracks = [dash_objects.CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
for i in range(dash_objects.NUM_SLOTS)]
tracks[0] = dash_objects.CameraObject(slot=0, object_id=9, d_rel=40.0, y_rel=0.0, is_lead_car=True, valid=True,
car_type=7, rotation=0)
msg = author.create(packer, 0, self.make_lead(prob=0.0), tracks, 1, 0.0)
assert self.payload(msg)[1] >> 3 == 0
def test_adjacent_car_forwarded_with_own_mux(self):
packer = CANPacker("honda_bosch_radarless_generated")
tracks = [dash_objects.CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
for i in range(dash_objects.NUM_SLOTS)]
tracks[3] = dash_objects.CameraObject(slot=3, object_id=12, d_rel=25.0, y_rel=3.0, is_lead_car=False, valid=True,
car_type=7, rotation=1)
msg = dash_objects.forward_hud_object(packer, 0, 20, tracks)
assert msg[1][0] >> 2 == 20
assert msg[1][1] >> 3 == 12
class TestCameraObjectTracker:
def test_tracks_persist_across_banks(self):
tracker = dash_objects.CameraObjectTracker()
class FakeParser:
vl_all = {"HUD_OBJECTS": {
"MUX": [2, 18], "OBJECT_ID": [5, 5], "LONG_DIST": [30.0, 31.0], "LAT_DIST": [1.0, 1.1],
"IS_LEAD_CAR": [0, 0], "CAR_TYPE": [7, 7], "ROTATION": [0, 0],
}}
tracker.update(FakeParser())
snap = tracker.snapshot()
assert snap[1].valid and snap[1].object_id == 5
assert snap[1].d_rel == 31.0
def test_empty_sentinel_invalid(self):
tracker = dash_objects.CameraObjectTracker()
class FakeParser:
vl_all = {"HUD_OBJECTS": {
"MUX": [1], "OBJECT_ID": [0], "LONG_DIST": [196.9], "LAT_DIST": [204.7],
"IS_LEAD_CAR": [0], "CAR_TYPE": [-1], "ROTATION": [-128],
}}
tracker.update(FakeParser())
assert not tracker.snapshot()[0].valid

18
artifacts/package_runtime/iqdbc/car/honda/tests/test_honda.py Normal file
View File

@@ -0,0 +1,18 @@
import re
from iqdbc.car.honda.fingerprints import FW_VERSIONS
from iqdbc.car.honda.values import HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL
HONDA_FW_VERSION_RE = br"[A-Z0-9]{5}(-|,)[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$"
class TestHondaFingerprint:
def test_fw_version_format(self):
# Asserts all FW versions follow an expected format
for fw_by_ecu in FW_VERSIONS.values():
for fws in fw_by_ecu.values():
for fw in fws:
assert re.match(HONDA_FW_VERSION_RE, fw) is not None, fw
def test_tja_bosch_only(self):
assert set(HONDA_BOSCH_TJA_CONTROL).issubset(set(HONDA_BOSCH)), "Nidec car found in TJA control list"

490
artifacts/package_runtime/iqdbc/car/honda/tests/test_radar_scan.py Normal file
View File

@@ -0,0 +1,490 @@
import math
import pytest
from iqdbc.can import CANParser
from iqdbc.car.honda.radar_scan import (AGE_RAW_INVALID, ALL_SCAN_ADDRS, BEARING_RAW_INVALID, BEARING_ZERO,
CLOSING_SPEED_RAW_INVALID, CLOSING_SPEED_RAW_ZERO,
CLOSING_SPEED_SIGMA_TRUST_MAX, DIST_BIAS_M, DIST_LSB_M,
DIST_RATIO_RAW_INVALID, DIST_RAW_INVALID, HondaRadarScanner,
QUIET_TIMEOUT_S, SCAN_DBC_NAME, SCAN_SLOTS, STATE_INVALID,
SWEEP_TRIGGER_ADDR, decode_closing_speed, decode_dist_ratio)
from iqdbc.dbc.generator.honda.honda_radar_scan import FRAME_SIGNALS, frame_address
BUS = 2
SWEEP_DT_NS = 66_000_000
def set_bits(data, start_bit, size, value):
value = int(value) & ((1 << size) - 1)
pos = start_bit
for i in range(size):
bit = (value >> (size - 1 - i)) & 1
byte_i, bit_i = pos // 8, pos % 8
if bit:
data[byte_i] |= (1 << bit_i)
pos = pos - 1 if bit_i > 0 else pos + 15
GEOMETRY = {kind: {name: (start, size) for name, start, size in sigs} for kind, sigs in FRAME_SIGNALS.items()}
def build_frame(slot, kind, **fields):
data = bytearray(8)
for name, value in fields.items():
set_bits(data, *GEOMETRY[kind][name], value)
return (frame_address(slot, kind), bytes(data), BUS)
def quartet(slot, cycle, dist_raw=1000, bearing_raw=BEARING_ZERO, state=1, dist_sigma=0, presence=40,
age=100, handle=5):
return [
build_frame(slot, "POS", SCAN_STATE=state, CYCLE=cycle, DIST_RAW=dist_raw, BEARING_RAW=bearing_raw,
DIST_SIGMA_RAW=dist_sigma),
build_frame(slot, "SHAPE", CYCLE=cycle, PRESENCE_RAW=presence),
build_frame(slot, "LIFE", CYCLE=cycle, AGE_RAW=age),
build_frame(slot, "IDENT", CYCLE=cycle, OBJECT_HANDLE=handle),
]
def motion_frame(slot, cycle, speed_raw=CLOSING_SPEED_RAW_ZERO, sigma_raw=0, ratio_raw=500):
return build_frame(slot, "MOTION", CYCLE=cycle, CLOSING_SPEED_RAW=speed_raw,
CLOSING_SPEED_SIGMA_RAW=sigma_raw, DIST_RATIO_RAW=ratio_raw)
def closing_sweep(slot_msgs, cycle):
# slot 15's quartet closes every sweep so the trigger fires
msgs = list(slot_msgs)
if not any(m[0] == SWEEP_TRIGGER_ADDR for m in msgs):
msgs += quartet(15, cycle, state=STATE_INVALID, dist_raw=DIST_RAW_INVALID,
bearing_raw=BEARING_RAW_INVALID, age=AGE_RAW_INVALID, handle=0)
return msgs
class ScanHarness:
def __init__(self):
self.scanner = object.__new__(HondaRadarScanner)
self.scanner.rcp = CANParser(SCAN_DBC_NAME, [(a, 15) for a in ALL_SCAN_ADDRS], BUS)
self.scanner.trigger_msg = SWEEP_TRIGGER_ADDR
self.scanner.pts = {}
self.scanner._ledgers = {}
self.scanner._slot_handles = [None] * SCAN_SLOTS
self.scanner._last_sweep_nanos = -1
self.updated = set()
self.nanos = 0
self.cycle = 0
def feed(self, msgs, dt_ns=SWEEP_DT_NS):
self.nanos += dt_ns
vls = self.scanner.rcp.update([self.nanos, list(msgs)])
self.updated.update(vls)
if self.scanner.trigger_msg not in self.updated:
if self.scanner.sweep_overdue():
return self.scanner.quiet_bus_radardata()
return None
result = self.scanner.process_sweep(self.updated)
self.updated.clear()
return result
def sweep(self, slot_msgs=(), cycle_step=1, dt_ns=SWEEP_DT_NS):
self.cycle = (self.cycle + cycle_step) & 0xF
return self.feed(closing_sweep(slot_msgs, self.cycle), dt_ns=dt_ns)
def object_sweep(self, slot=0, handle=5, dist_raw=1000, with_motion=True, cycle_step=1, age_step=None,
dt_ns=SWEEP_DT_NS, **kwargs):
if age_step is None:
age_step = 2 * cycle_step
self._age = (getattr(self, "_age", 100) + age_step) & 0xFFF
cycle = (self.cycle + cycle_step) & 0xF
msgs = quartet(slot, cycle, dist_raw=dist_raw, age=self._age, handle=handle, **kwargs)
if with_motion:
msgs.append(motion_frame(slot, cycle))
return self.sweep(msgs, cycle_step=cycle_step, dt_ns=dt_ns)
class TestFieldDecoding:
def test_dist_conversion(self):
assert DIST_LSB_M * 1000 + DIST_BIAS_M == pytest.approx(54.12)
def test_closing_speed_decode_and_domain(self):
assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO) == 0.0
assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO + 64) == 1.0
assert decode_closing_speed(CLOSING_SPEED_RAW_INVALID) is None
assert decode_closing_speed(1729) is None
assert decode_closing_speed(None) is None
def test_closing_speed_sigma_veto(self):
assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO, CLOSING_SPEED_SIGMA_TRUST_MAX) == 0.0
assert decode_closing_speed(CLOSING_SPEED_RAW_ZERO, CLOSING_SPEED_SIGMA_TRUST_MAX + 1) is None
def test_dist_ratio_decode(self):
assert decode_dist_ratio(500) == pytest.approx(1.0)
assert decode_dist_ratio(DIST_RATIO_RAW_INVALID) is None
assert decode_dist_ratio(None) is None
def test_bearing_sign_convention(self):
h = ScanHarness()
h.object_sweep(bearing_raw=BEARING_ZERO + 100)
result = h.object_sweep(bearing_raw=BEARING_ZERO + 100)
assert result.points[0].yRel > 0 # left of center is positive
dist = result.points[0].dRel
assert result.points[0].yRel == pytest.approx(dist * math.tan(100 / 2048))
def test_bearing_right_of_center_is_negative(self):
h = ScanHarness()
h.object_sweep(bearing_raw=BEARING_ZERO - 100)
result = h.object_sweep(bearing_raw=BEARING_ZERO - 100)
assert result.points[0].yRel < 0
def test_boresight_is_zero(self):
h = ScanHarness()
h.object_sweep(bearing_raw=BEARING_ZERO)
result = h.object_sweep(bearing_raw=BEARING_ZERO)
assert result.points[0].yRel == 0.0
class TestPublicationRules:
def test_birth_is_withheld_until_second_observation(self):
h = ScanHarness()
result = h.object_sweep()
assert len(result.points) == 0
result = h.object_sweep()
assert len(result.points) == 1
point = result.points[0]
assert point.trackId == 5
assert point.measured
assert math.isnan(point.aRel) and math.isnan(point.yvRel)
def test_handle_is_wire_identity_not_synthetic(self):
h = ScanHarness()
h.object_sweep(handle=0x22)
result = h.object_sweep(handle=0x22)
assert result.points[0].trackId == 0x22
@pytest.mark.parametrize("field,value", [("state", STATE_INVALID), ("dist_raw", DIST_RAW_INVALID),
("bearing_raw", BEARING_RAW_INVALID)])
def test_sentinels_invalidate_observation(self, field, value):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
kwargs = {field: value}
result = h.object_sweep(**kwargs)
assert len(result.points) == 0
def test_age_sentinel_invalidates_observation(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
cycle = (h.cycle + 1) & 0xF
msgs = quartet(0, cycle, age=AGE_RAW_INVALID, handle=5) + [motion_frame(0, cycle)]
result = h.sweep(msgs)
assert len(result.points) == 0
@pytest.mark.parametrize("handle", [0, 0x40, 0xFF])
def test_out_of_range_handle_invalidates(self, handle):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
result = h.object_sweep(handle=handle)
assert len(result.points) == 0
def test_incomplete_quartet_is_not_an_observation(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
cycle = (h.cycle + 1) & 0xF
h._age = (h._age + 2) & 0xFFF
msgs = quartet(0, cycle, age=h._age, handle=5)[:3] # drop IDENT
result = h.sweep(msgs)
# a dropped CAN frame is not a lifecycle event: the published point persists untouched
assert len(result.points) == 1
result = h.object_sweep()
assert len(result.points) == 1
assert result.points[0].measured
def test_cycle_mismatch_across_quartet_is_incoherent(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
cycle = (h.cycle + 1) & 0xF
msgs = quartet(0, cycle, age=200, handle=5)
bad_life = build_frame(0, "LIFE", CYCLE=(cycle + 1) & 0xF, AGE_RAW=200)
msgs[2] = bad_life
result = h.sweep(msgs)
# an incoherent quartet is not an observation: the published point persists untouched
assert len(result.points) == 1
assert result.points[0].measured
class TestLifecycle:
def test_age_advances_two_per_cycle_keeps_identity(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
result = h.object_sweep()
assert len(result.points) == 1
def test_continuity_across_skipped_cycles(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
result = h.object_sweep(cycle_step=3, age_step=6)
assert len(result.points) == 1
def test_cycle_and_age_wraparound_stay_same_incarnation(self):
h = ScanHarness()
h.cycle = 14
h._age = 4094
h.object_sweep() # cycle 15, age 4094+2 wraps
h.object_sweep() # cycle 0
result = h.object_sweep()
assert len(result.points) == 1
def test_lifecycle_break_starts_new_incarnation(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
# same handle, age jumps arbitrarily: history must not carry over, so no publication this sweep
result = h.object_sweep(age_step=500)
assert len(result.points) == 0
result = h.object_sweep()
assert len(result.points) == 1
def test_death_then_rebirth_reuses_handle_with_clean_history(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
for _ in range(4):
h.sweep() # object absent long enough to expire its ledger
result = h.object_sweep()
assert len(result.points) == 0
result = h.object_sweep()
assert len(result.points) == 1
class TestMotionPolicy:
def test_native_speed_is_published(self):
h = ScanHarness()
speed_raw = CLOSING_SPEED_RAW_ZERO + 128
cycle = (h.cycle + 1) & 0xF
h.sweep(quartet(0, cycle, age=100, handle=5) + [motion_frame(0, cycle, speed_raw=speed_raw)])
cycle = (h.cycle + 1) & 0xF
result = h.sweep(quartet(0, cycle, age=102, handle=5) + [motion_frame(0, cycle, speed_raw=speed_raw)])
assert result.points[0].vRel == pytest.approx(2.0)
assert result.points[0].measured
def test_missing_motion_frame_never_invalidates_geometry(self):
h = ScanHarness()
h.object_sweep(with_motion=False)
result = h.object_sweep(with_motion=False)
# without any motion source and no held speed, the point is withheld rather than synthesized
assert len(result.points) == 0
def test_stale_motion_cycle_is_ignored(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
cycle = (h.cycle + 1) & 0xF
h._age = (h._age + 2) & 0xFFF
msgs = quartet(0, cycle, age=h._age, handle=5) + [motion_frame(0, (cycle - 1) & 0xF)]
result = h.sweep(msgs)
# motion from another cycle contributes nothing: coasts on held speed, unmeasured
assert len(result.points) == 1
assert not result.points[0].measured
def test_high_sigma_speed_coasts_instead_of_synthesizing(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
cycle = (h.cycle + 1) & 0xF
h._age = (h._age + 2) & 0xFFF
msgs = quartet(0, cycle, age=h._age, handle=5) + \
[motion_frame(0, cycle, sigma_raw=CLOSING_SPEED_SIGMA_TRUST_MAX + 1)]
result = h.sweep(msgs)
assert len(result.points) == 1
assert not result.points[0].measured
assert result.points[0].vRel == pytest.approx(0.0) # the held speed, not a derivative
def test_ratio_field_supplies_speed_when_native_missing(self):
h = ScanHarness()
dist_raw = 1000
cycle = (h.cycle + 1) & 0xF
h.sweep(quartet(0, cycle, dist_raw=dist_raw, age=100, handle=5) +
[motion_frame(0, cycle, speed_raw=CLOSING_SPEED_RAW_INVALID, ratio_raw=490)])
cycle = (h.cycle + 1) & 0xF
result = h.sweep(quartet(0, cycle, dist_raw=dist_raw, age=102, handle=5) +
[motion_frame(0, cycle, speed_raw=CLOSING_SPEED_RAW_INVALID, ratio_raw=490)])
assert len(result.points) == 1
dist = DIST_LSB_M * dist_raw + DIST_BIAS_M
dt = SWEEP_DT_NS * 1e-9
assert result.points[0].vRel == pytest.approx(dist * (1.0 - 0.99) / dt)
assert result.points[0].measured
def test_fast_clean_range_rate_without_sources_is_withheld(self):
h = ScanHarness()
h.object_sweep(with_motion=False, dist_raw=1000)
# large clean jump with no motion evidence: raw-rate limit rejects the range outright
result = h.object_sweep(with_motion=False, dist_raw=3000)
assert len(result.points) == 0
class TestRangeAcceptance:
def test_discontinuity_is_rejected_and_never_becomes_baseline(self):
h = ScanHarness()
h.object_sweep(dist_raw=1000)
h.object_sweep(dist_raw=1002)
# jump far beyond the hard innovation gate while claiming zero closing speed
result = h.object_sweep(dist_raw=3000)
assert len(result.points) == 1
assert not result.points[0].measured
# the rejected range did not become the derivative baseline: returning to the
# consistent range publishes measured again
result = h.object_sweep(dist_raw=1004)
assert result.points[0].measured
def test_small_innovation_accepted(self):
h = ScanHarness()
h.object_sweep(dist_raw=1000)
result = h.object_sweep(dist_raw=1005)
assert result.points[0].measured
class TestSlotsAndIdentity:
def test_slot_migration_preserves_identity(self):
h = ScanHarness()
h.object_sweep(slot=2)
h.object_sweep(slot=2)
result = h.object_sweep(slot=9)
assert len(result.points) == 1
assert result.points[0].trackId == 5
def test_duplicate_identity_prefers_bound_slot(self):
h = ScanHarness()
h.object_sweep(slot=2, dist_raw=1000)
h.object_sweep(slot=2, dist_raw=1002)
cycle = (h.cycle + 1) & 0xF
h._age = (h._age + 2) & 0xFFF
msgs = quartet(2, cycle, dist_raw=1004, age=h._age, handle=5) + [motion_frame(2, cycle)] + \
quartet(9, cycle, dist_raw=2000, age=h._age, handle=5) + [motion_frame(9, cycle)]
result = h.sweep(msgs)
assert len(result.points) == 1
assert result.points[0].dRel == pytest.approx(DIST_LSB_M * 1004 + DIST_BIAS_M)
def test_slot_replacement_hides_old_occupant(self):
h = ScanHarness()
h.object_sweep(slot=3, handle=7)
h.object_sweep(slot=3, handle=7)
# a different identity takes the slot; the old one is hidden but not destroyed
result = h.object_sweep(slot=3, handle=9, age_step=333)
assert all(p.trackId != 7 for p in result.points)
def test_one_identity_never_two_points(self):
h = ScanHarness()
cycle = (h.cycle + 1) & 0xF
msgs = quartet(1, cycle, age=100, handle=5) + [motion_frame(1, cycle)] + \
quartet(6, cycle, age=100, handle=5) + [motion_frame(6, cycle)]
h.sweep(msgs)
cycle = (h.cycle + 1) & 0xF
msgs = quartet(1, cycle, age=102, handle=5) + [motion_frame(1, cycle)] + \
quartet(6, cycle, age=102, handle=5) + [motion_frame(6, cycle)]
result = h.sweep(msgs)
assert len(result.points) == 1
class TestBusSilence:
def test_quiet_bus_publishes_empty_not_none(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
result = None
for _ in range(30):
result = h.feed([], dt_ns=10_000_000)
if result is not None:
break
assert result is not None
assert result.errors.radarUnavailableTemporary
assert len(result.points) == 0
def test_recovery_after_silence_starts_fresh(self):
h = ScanHarness()
h.object_sweep()
h.object_sweep()
for _ in range(30):
if h.feed([], dt_ns=10_000_000) is not None:
break
result = h.object_sweep()
assert len(result.points) == 0
result = h.object_sweep()
assert len(result.points) == 1
def test_no_stale_publication_before_first_sweep(self):
h = ScanHarness()
for _ in range(50):
assert h.feed([], dt_ns=10_000_000) is None
class TestQuietTimeoutValue:
def test_timeout_is_about_three_sweeps(self):
assert QUIET_TIMEOUT_S == pytest.approx(3 / 15, abs=0.01)
class TestScanInterfaceGating:
def build(self, candidate, alpha_long=False, docs=False):
from iqdbc.car import gen_empty_fingerprint
from iqdbc.car.honda.interface import CarInterface
CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], alpha_long, False, docs)
return CP
def test_verified_platform_has_radar(self):
from iqdbc.car.honda.values import CAR
for car in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_ACCORD, CAR.HONDA_CRV_5G):
assert not self.build(car).radarUnavailable
def test_radar_survives_openpilot_longitudinal(self):
from iqdbc.car.honda.values import CAR
CP = self.build(CAR.HONDA_CIVIC_BOSCH, alpha_long=True)
assert CP.openpilotLongitudinalControl
assert not CP.radarUnavailable
def test_unverified_family_platform_stays_off(self):
from iqdbc.car.honda.values import CAR
for car in (CAR.HONDA_E, CAR.HONDA_INSIGHT, CAR.HONDA_NBOX_2G, CAR.ACURA_RDX_3G, CAR.HONDA_CRV_HYBRID):
assert self.build(car).radarUnavailable
def test_radarless_and_canfd_stay_off(self):
from iqdbc.car.honda.values import CAR
assert self.build(CAR.HONDA_CIVIC_2022).radarUnavailable
assert self.build(CAR.HONDA_CRV_6G).radarUnavailable
def test_docs_never_claim_radar(self):
from iqdbc.car.honda.values import CAR
assert self.build(CAR.HONDA_CIVIC_BOSCH, docs=True).radarUnavailable
def test_radar_interface_routes_scanner(self):
from iqdbc.car import gen_empty_fingerprint
from iqdbc.car.honda.interface import CarInterface
from iqdbc.car.honda.values import CAR
CP = self.build(CAR.HONDA_CIVIC_BOSCH)
CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], False, False, False)
ri = CarInterface.RadarInterface(CP, CP_IQ)
assert ri.scanner is not None
assert ri.trigger_msg == SWEEP_TRIGGER_ADDR
def test_radar_interface_keeps_nidec_path(self):
from iqdbc.car import gen_empty_fingerprint
from iqdbc.car.honda.interface import CarInterface
from iqdbc.car.honda.values import CAR
CP = self.build(CAR.HONDA_CIVIC)
CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_CIVIC, gen_empty_fingerprint(), [], False, False, False)
ri = CarInterface.RadarInterface(CP, CP_IQ)
assert ri.scanner is None
assert ri.trigger_msg == 0x445
def test_radar_interface_sleeps_when_unavailable(self):
from iqdbc.car import gen_empty_fingerprint
from iqdbc.car.honda.interface import CarInterface
from iqdbc.car.honda.values import CAR
CP = self.build(CAR.HONDA_E)
CP_IQ = CarInterface.get_params_iq(CP, CAR.HONDA_E, gen_empty_fingerprint(), [], False, False, False)
ri = CarInterface.RadarInterface(CP, CP_IQ)
assert ri.scanner is None and ri.rcp is None

84
artifacts/package_runtime/iqdbc/car/honda/tests/test_radar_scan_dbc.py Normal file
View File

@@ -0,0 +1,84 @@
from iqdbc.can.dbc import DBC as DbcFile
from iqdbc.car import Bus
from iqdbc.car.honda.values import CAR, DBC, HONDA_RADAR_SCAN_CAPABLE, HONDA_RADAR_SCAN_VERIFIED
from iqdbc.dbc.generator.honda.honda_radar_scan import (FRAME_SIGNALS, QUARTET_KINDS, SCAN_SLOTS,
frame_address, motion_address, quartet_base_address)
SCAN_DBC_NAME = 'honda_radar_scan_generated'
class TestScanAddressing:
def test_quartet_bases(self):
assert [quartet_base_address(s) for s in range(SCAN_SLOTS)] == \
[0x280, 0x284, 0x288, 0x28C, 0x2D0, 0x2D4, 0x2D8, 0x2DC, 0x2E0, 0x2E4, 0x2E8, 0x2EC, 0x2F0, 0x2F4, 0x2F8, 0x2FC]
def test_motion_addresses(self):
assert [motion_address(s) for s in range(SCAN_SLOTS)] == \
[0x2C8, 0x2C9, 0x2CA, 0x2CB, 0x2CC, 0x2CD, 0x2CE, 0x2CF, 0x290, 0x291, 0x292, 0x293, 0x294, 0x295, 0x296, 0x297]
def test_eighty_unique_addresses(self):
addrs = [frame_address(s, k) for s in range(SCAN_SLOTS) for k in (*QUARTET_KINDS, "MOTION")]
assert len(addrs) == 80
assert len(set(addrs)) == 80
def test_quartet_kind_order(self):
for slot in range(SCAN_SLOTS):
base = quartet_base_address(slot)
assert [frame_address(slot, k) for k in QUARTET_KINDS] == [base, base + 1, base + 2, base + 3]
class TestScanDbcGeometry:
def setup_method(self):
self.dbc = DbcFile(SCAN_DBC_NAME)
def geometry(self, addr):
msg = self.dbc.addr_to_msg[addr]
return {sig.name: (sig.start_bit, sig.size) for sig in msg.sigs.values()}
def test_every_frame_present_with_size_8(self):
for slot in range(SCAN_SLOTS):
for kind in (*QUARTET_KINDS, "MOTION"):
msg = self.dbc.addr_to_msg[frame_address(slot, kind)]
assert msg.name == f"RADAR_SCAN_{slot:02d}_{kind}"
assert msg.size == 8
def test_bit_geometry_matches_spec(self):
expected = {kind: {name: (start, size) for name, start, size in sigs} for kind, sigs in FRAME_SIGNALS.items()}
for slot in range(SCAN_SLOTS):
for kind in (*QUARTET_KINDS, "MOTION"):
assert self.geometry(frame_address(slot, kind)) == expected[kind], (slot, kind)
def test_pos_frame_field_widths(self):
geo = self.geometry(frame_address(0, "POS"))
assert geo["DIST_RAW"] == (23, 12)
assert geo["BEARING_RAW"] == (39, 11)
assert geo["SCAN_STATE"] == (15, 4)
assert geo["DIST_SIGMA_RAW"] == (7, 7)
def test_ident_handle_is_byte_six(self):
geo = self.geometry(frame_address(0, "IDENT"))
assert geo["OBJECT_HANDLE"] == (55, 8)
def test_motion_field_widths(self):
geo = self.geometry(frame_address(0, "MOTION"))
assert geo["CLOSING_SPEED_RAW"] == (7, 11)
assert geo["CLOSING_SPEED_SIGMA_RAW"] == (23, 10)
assert geo["DIST_RATIO_RAW"] == (55, 10)
def test_cycle_positions_per_kind(self):
positions = {"POS": (27, 4), "SHAPE": (28, 4), "LIFE": (11, 4), "IDENT": (12, 4), "MOTION": (12, 4)}
for kind, expected in positions.items():
assert self.geometry(frame_address(3, kind))["CYCLE"] == expected
class TestScanPlatformWiring:
def test_scan_dbc_on_exactly_the_capable_family(self):
for car in CAR:
has_scan_dbc = DBC[car].get(Bus.radar) == SCAN_DBC_NAME
assert has_scan_dbc == (car in HONDA_RADAR_SCAN_CAPABLE), car
def test_verified_platforms_are_capable(self):
assert HONDA_RADAR_SCAN_VERIFIED <= HONDA_RADAR_SCAN_CAPABLE
def test_verified_set(self):
assert HONDA_RADAR_SCAN_VERIFIED == {CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G}

468
artifacts/package_runtime/iqdbc/car/honda/values.py Normal file
View File

@@ -0,0 +1,468 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag
from iqdbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, SupportType
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
Ecu = structs.CarParams.Ecu
VisualAlert = structs.CarControl.HUDControl.VisualAlert
GearShifter = structs.CarState.GearShifter
class CarControllerParams:
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
# perform the closed loop control, and might need some
# to apply some more braking if we're on a downhill slope.
# Our controller should still keep the 2 second average above
# -3.5 m/s^2 as per planner limits
NIDEC_ACCEL_MIN = -4.0 # m/s^2
NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons
NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6]
NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0]
NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6]
NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.]
NIDEC_GAS_MAX = 198 # 0xc6
NIDEC_BRAKE_MAX = 1024 // 4
BOSCH_ACCEL_MIN = -3.5 # m/s^2
BOSCH_ACCEL_MAX = 2.0 # m/s^2
BOSCH_GAS_LOOKUP_BP = [0.0, 2.0] # 2m/s^2
BOSCH_GAS_LOOKUP_V = [0, 1600]
STEER_STEP = 1 # 100 Hz
STEER_DELTA_UP = 3 # min/max in 0.33s for all Honda
STEER_DELTA_DOWN = 3
STEER_GLOBAL_MIN_SPEED = 3 * CV.MPH_TO_MS
def __init__(self, CP):
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
# mirror of list (assuming first item is zero) for interp of signed request
# values and verify that both arrays begin at zero
assert CP.lateralParams.torqueBP[0] == 0
assert CP.lateralParams.torqueV[0] == 0
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
class HondaSafetyFlags(IntFlag):
ALT_BRAKE = 1
BOSCH_LONG = 2
NIDEC_ALT = 4
RADARLESS = 8
BOSCH_CANFD = 16
class HondaFlags(IntFlag):
# Detected flags
# Bosch models with alternate set of LKAS_HUD messages
BOSCH_EXT_HUD = 1
BOSCH_ALT_BRAKE = 2
# Static flags
BOSCH = 4
BOSCH_RADARLESS = 8
NIDEC = 16
NIDEC_ALT_PCM_ACCEL = 32
NIDEC_ALT_SCM_MESSAGES = 64
BOSCH_CANFD = 128
HAS_ALL_DOOR_STATES = 256 # Some Hondas have all door states, others only driver door
BOSCH_ALT_RADAR = 512
ALLOW_MANUAL_TRANS = 1024
HYBRID = 2048
BOSCH_TJA_CONTROL = 4096
# Car button codes
class CruiseButtons:
RES_ACCEL = 4
DECEL_SET = 3
CANCEL = 2
MAIN = 1
class CruiseSettings:
DISTANCE = 3
LKAS = 1
@dataclass
class HondaCarDocs(CarDocs):
package: str = "Honda Sensing"
def init_make(self, CP: structs.CarParams):
if CP.flags & HondaFlags.BOSCH:
if CP.flags & HondaFlags.BOSCH_CANFD:
harness = CarHarness.bosch_c
elif CP.flags & HondaFlags.BOSCH_RADARLESS:
harness = CarHarness.bosch_b
else:
harness = CarHarness.bosch_a
else:
harness = CarHarness.nidec
self.car_parts = CarParts.common([harness])
if CP.carFingerprint in (CAR.HONDA_CLARITY,):
self.car_parts = CarParts.common([CarHarness.honda_clarity])
self.car_parts.custom_parts_url = "https://shop.retropilot.org/product/honda-clarity-proxy-board-kit"
self.support_type: SupportType = SupportType.COMMUNITY
self.support_link: str = "community"
class Footnote(Enum):
CIVIC_DIESEL = CarFootnote(
"2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.",
Column.FSR_STEERING)
@dataclass
class HondaBoschPlatformConfig(PlatformConfig):
def init(self):
self.flags |= HondaFlags.BOSCH
@dataclass
class HondaBoschCANFDPlatformConfig(HondaBoschPlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'honda_common_canfd_generated', Bus.radar: 'honda_common_canfd_generated'})
def init(self):
super().init()
self.flags |= HondaFlags.BOSCH_CANFD
@dataclass
class HondaNidecPlatformConfig(PlatformConfig):
def init(self):
self.flags |= HondaFlags.NIDEC
def radar_dbc_dict(pt_dict):
return {Bus.pt: pt_dict, Bus.radar: 'acura_ilx_2016_nidec'}
# Certain Hondas have an extra steering sensor at the bottom of the steering rack,
# which improves controls quality as it removes the steering column torsion from feedback.
# Tire stiffness factor fictitiously lower if it includes the steering column torsion effect.
# For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani"
class CAR(Platforms):
# Bosch Cars
HONDA_NBOX_2G = HondaBoschPlatformConfig(
[
HondaCarDocs("Honda N-Box 2018", "All", min_steer_speed=5.),
],
CarSpecs(mass=890., wheelbase=2.520, steerRatio=18.64),
{Bus.pt: 'acura_rdx_2020_can_generated', Bus.radar: 'honda_radar_scan_generated'},
)
HONDA_ACCORD = HondaBoschPlatformConfig(
[
HondaCarDocs("Honda Accord 2018-22", "All", video="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS),
HondaCarDocs("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS),
HondaCarDocs("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
],
# steerRatio: 11.82 is spec end-to-end
CarSpecs(mass=3279 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=16.33, centerToFrontRatio=0.39, tireStiffnessFactor=0.8467),
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.radar: 'honda_radar_scan_generated'},
flags=HondaFlags.ALLOW_MANUAL_TRANS,
)
HONDA_ACCORD_11G = HondaBoschCANFDPlatformConfig(
[
HondaCarDocs("Honda Accord 2023-25", "All"),
HondaCarDocs("Honda Accord Hybrid 2023-25", "All"),
],
CarSpecs(mass=3477 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=16.0, centerToFrontRatio=0.39),
)
HONDA_CIVIC_BOSCH = HondaBoschPlatformConfig(
[
HondaCarDocs("Honda Civic 2019-21", "All", video="https://www.youtube.com/watch?v=4Iz1Mz5LGF8",
footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS),
HondaCarDocs("Honda Civic Hatchback 2017-18", min_steer_speed=12. * CV.MPH_TO_MS),
HondaCarDocs("Honda Civic Hatchback 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS),
],
CarSpecs(mass=1326, wheelbase=2.7, steerRatio=15.38, centerToFrontRatio=0.4), # steerRatio: 10.93 is end-to-end spec
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.radar: 'honda_radar_scan_generated'},
flags=HondaFlags.ALLOW_MANUAL_TRANS,
)
HONDA_CIVIC_BOSCH_DIESEL = HondaBoschPlatformConfig(
[], # don't show in docs
HONDA_CIVIC_BOSCH.specs,
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.radar: 'honda_radar_scan_generated'},
)
HONDA_CIVIC_2022 = HondaBoschPlatformConfig(
[
HondaCarDocs("Honda Civic 2022-24", "All", video="https://youtu.be/ytiOT5lcp6Q"),
HondaCarDocs("Honda Civic Hybrid 2025-26", "All"),
HondaCarDocs("Honda Civic Hatchback 2022-24", "All", video="https://youtu.be/ytiOT5lcp6Q"),
HondaCarDocs("Honda Civic Hatchback Hybrid (Europe only) 2023", "All"),
# TODO: Confirm 2024
HondaCarDocs("Honda Civic Hatchback Hybrid 2025-26", "All"),
],
HONDA_CIVIC_BOSCH.specs,
{Bus.pt: 'honda_bosch_radarless_generated'},
flags=HondaFlags.BOSCH_RADARLESS | HondaFlags.ALLOW_MANUAL_TRANS
)
HONDA_CRV_5G = HondaBoschPlatformConfig(
[HondaCarDocs("Honda CR-V 2017-22", min_steer_speed=15. * CV.MPH_TO_MS)],
# steerRatio: 12.3 is spec end-to-end
CarSpecs(mass=3410 * CV.LB_TO_KG, wheelbase=2.66, steerRatio=16.0, centerToFrontRatio=0.41, tireStiffnessFactor=0.677),
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.body: 'honda_crv_ex_2017_body_generated',
Bus.radar: 'honda_radar_scan_generated'},
flags=HondaFlags.BOSCH_ALT_BRAKE,
)
HONDA_CRV_6G = HondaBoschCANFDPlatformConfig(
[
HondaCarDocs("Honda CR-V 2023-26", "All"),
HondaCarDocs("Honda CR-V Hybrid 2023-26", "All"),
],
CarSpecs(mass=1703, wheelbase=2.7, steerRatio=16.2, centerToFrontRatio=0.42),
)
HONDA_CRV_HYBRID = HondaBoschPlatformConfig(
[HondaCarDocs("Honda CR-V Hybrid 2017-22", min_steer_speed=12. * CV.MPH_TO_MS)],
# mass: mean of 4 models in kg, steerRatio: 12.3 is spec end-to-end
CarSpecs(mass=1667, wheelbase=2.66, steerRatio=16, centerToFrontRatio=0.41, tireStiffnessFactor=0.677),
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated', Bus.radar: 'honda_radar_scan_generated'},
)
HONDA_HRV_3G = HondaBoschPlatformConfig(
[HondaCarDocs("Honda HR-V 2023-25", "All")],
CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.61, steerRatio=15.2, centerToFrontRatio=0.41, tireStiffnessFactor=0.5),
{Bus.pt: 'honda_bosch_radarless_generated'},
flags=HondaFlags.BOSCH_RADARLESS,
)
HONDA_CITY_7G = HondaBoschPlatformConfig(
[HondaCarDocs("Honda City (Brazil only) 2023", "All")],
CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.6, steerRatio=19.0, centerToFrontRatio=0.41, minSteerSpeed=23. * CV.KPH_TO_MS),
{Bus.pt: 'honda_bosch_radarless_generated'},
flags=HondaFlags.BOSCH_RADARLESS,
)
ACURA_RDX_3G = HondaBoschPlatformConfig(
[HondaCarDocs("Acura RDX 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS)],
CarSpecs(mass=4068 * CV.LB_TO_KG, wheelbase=2.75, steerRatio=11.95, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), # as spec
{Bus.pt: 'acura_rdx_2020_can_generated', Bus.radar: 'honda_radar_scan_generated'},
flags=HondaFlags.BOSCH_ALT_BRAKE,
)
HONDA_INSIGHT = HondaBoschPlatformConfig(
[HondaCarDocs("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)],
CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.0, centerToFrontRatio=0.39, tireStiffnessFactor=0.82), # as spec
{Bus.pt: 'honda_insight_ex_2019_can_generated', Bus.radar: 'honda_radar_scan_generated'},
)
HONDA_E = HondaBoschPlatformConfig(
[HondaCarDocs("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS)],
CarSpecs(mass=3338.8 * CV.LB_TO_KG, wheelbase=2.5, centerToFrontRatio=0.5, steerRatio=16.71, tireStiffnessFactor=0.82),
{Bus.pt: 'acura_rdx_2020_can_generated', Bus.radar: 'honda_radar_scan_generated'},
)
HONDA_PILOT_4G = HondaBoschCANFDPlatformConfig(
[HondaCarDocs("Honda Pilot 2023-25", "All")],
CarSpecs(mass=4660 * CV.LB_TO_KG, wheelbase=2.89, centerToFrontRatio=0.442, steerRatio=17.5),
)
HONDA_PASSPORT_4G = HondaBoschCANFDPlatformConfig(
[HondaCarDocs("Honda Passport 2026", "All")],
CarSpecs(mass=4620 * CV.LB_TO_KG, wheelbase=2.89, centerToFrontRatio=0.442, steerRatio=18.5),
)
# mid-model refresh
ACURA_MDX_4G_MMR = HondaBoschCANFDPlatformConfig(
[HondaCarDocs("Acura MDX 2025-26", "All except Type S")],
CarSpecs(mass=4544 * CV.LB_TO_KG, wheelbase=2.89, centerToFrontRatio=0.428, steerRatio=16.2),
)
HONDA_ODYSSEY_5G_MMR = HondaBoschPlatformConfig(
[HondaCarDocs("Honda Odyssey 2021-26", "All", min_steer_speed=70. * CV.KPH_TO_MS)],
CarSpecs(mass=4590 * CV.LB_TO_KG, wheelbase=3.00, steerRatio=19.4, centerToFrontRatio=0.41),
{Bus.pt: 'acura_rdx_2020_can_generated'},
flags=HondaFlags.BOSCH_ALT_BRAKE | HondaFlags.BOSCH_ALT_RADAR,
)
ACURA_TLX_2G = HondaBoschPlatformConfig(
[HondaCarDocs("Acura TLX 2021", "All")],
CarSpecs(mass=3982 * CV.LB_TO_KG, wheelbase=2.87, steerRatio=14.0, centerToFrontRatio=0.43),
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated'},
flags=HondaFlags.BOSCH_ALT_RADAR,
)
# mid-model refresh
ACURA_TLX_2G_MMR = HondaBoschCANFDPlatformConfig(
[HondaCarDocs("Acura TLX 2025", "All")],
CarSpecs(mass=3990 * CV.LB_TO_KG, wheelbase=2.87, centerToFrontRatio=0.43, steerRatio=13.7),
)
# Nidec Cars
ACURA_ILX = HondaNidecPlatformConfig(
[
HondaCarDocs("Acura ILX 2016-18", "Technology Plus Package or AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS),
HondaCarDocs("Acura ILX 2019", "All", min_steer_speed=25. * CV.MPH_TO_MS),
],
CarSpecs(mass=3095 * CV.LB_TO_KG, wheelbase=2.67, steerRatio=18.61, centerToFrontRatio=0.37, tireStiffnessFactor=0.72), # 15.3 is spec end-to-end
radar_dbc_dict('acura_ilx_2016_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES | HondaFlags.HAS_ALL_DOOR_STATES,
)
HONDA_CRV = HondaNidecPlatformConfig(
[HondaCarDocs("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS)],
CarSpecs(mass=3572 * CV.LB_TO_KG, wheelbase=2.62, steerRatio=16.89, centerToFrontRatio=0.41, tireStiffnessFactor=0.444), # as spec
radar_dbc_dict('honda_crv_touring_2016_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES | HondaFlags.HAS_ALL_DOOR_STATES,
)
HONDA_CRV_EU = HondaNidecPlatformConfig(
[], # Euro version of CRV Touring, don't show in docs
HONDA_CRV.specs,
radar_dbc_dict('honda_crv_touring_2016_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES | HondaFlags.HAS_ALL_DOOR_STATES,
)
HONDA_FIT = HondaNidecPlatformConfig(
[HondaCarDocs("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS)],
CarSpecs(mass=2644 * CV.LB_TO_KG, wheelbase=2.53, steerRatio=13.06, centerToFrontRatio=0.39, tireStiffnessFactor=0.75),
radar_dbc_dict('acura_ilx_2016_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES,
)
HONDA_FREED = HondaNidecPlatformConfig(
[HondaCarDocs("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS)],
CarSpecs(mass=3086. * CV.LB_TO_KG, wheelbase=2.74, steerRatio=13.06, centerToFrontRatio=0.39, tireStiffnessFactor=0.75), # mostly copied from FIT
radar_dbc_dict('acura_ilx_2016_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES,
)
HONDA_HRV = HondaNidecPlatformConfig(
[HondaCarDocs("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS)],
HONDA_HRV_3G.specs,
radar_dbc_dict('acura_ilx_2016_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES,
)
HONDA_ODYSSEY = HondaNidecPlatformConfig(
[HondaCarDocs("Honda Odyssey 2018-20")],
CarSpecs(mass=1900, wheelbase=3.0, steerRatio=14.35, centerToFrontRatio=0.41, tireStiffnessFactor=0.82),
radar_dbc_dict('honda_odyssey_exl_2018_generated'),
flags=HondaFlags.NIDEC_ALT_PCM_ACCEL | HondaFlags.HAS_ALL_DOOR_STATES,
)
HONDA_ODYSSEY_TWN = HondaNidecPlatformConfig(
[HondaCarDocs("Honda Odyssey (Taiwan) 2018-19")],
CarSpecs(mass=1865, wheelbase=2.9, steerRatio=14.35, centerToFrontRatio=0.44, tireStiffnessFactor=0.82),
radar_dbc_dict('honda_odyssey_twn_2018_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES,
)
ACURA_RDX = HondaNidecPlatformConfig(
[HondaCarDocs("Acura RDX 2016-18", "AcuraWatch Plus or Advance Package", min_steer_speed=12. * CV.MPH_TO_MS)],
CarSpecs(mass=3925 * CV.LB_TO_KG, wheelbase=2.68, steerRatio=15.0, centerToFrontRatio=0.38, tireStiffnessFactor=0.444), # as spec
radar_dbc_dict('acura_rdx_2018_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES | HondaFlags.HAS_ALL_DOOR_STATES,
)
HONDA_PILOT = HondaNidecPlatformConfig(
[
HondaCarDocs("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS),
HondaCarDocs("Honda Passport 2019-25", "All", min_steer_speed=12. * CV.MPH_TO_MS),
],
HONDA_PILOT_4G.specs,
radar_dbc_dict('acura_ilx_2016_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES | HondaFlags.HAS_ALL_DOOR_STATES,
)
HONDA_RIDGELINE = HondaNidecPlatformConfig(
[HondaCarDocs("Honda Ridgeline 2017-25", min_steer_speed=12. * CV.MPH_TO_MS)],
CarSpecs(mass=4515 * CV.LB_TO_KG, wheelbase=3.18, centerToFrontRatio=0.41, steerRatio=15.59, tireStiffnessFactor=0.444), # as spec
radar_dbc_dict('acura_ilx_2016_can_generated'),
flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES | HondaFlags.HAS_ALL_DOOR_STATES,
)
HONDA_CIVIC = HondaNidecPlatformConfig(
[HondaCarDocs("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video="https://youtu.be/-IkImTe1NYE")],
CarSpecs(mass=1326, wheelbase=2.70, centerToFrontRatio=0.4, steerRatio=15.38), # 10.93 is end-to-end spec
radar_dbc_dict('honda_civic_touring_2016_can_generated'),
flags=HondaFlags.HAS_ALL_DOOR_STATES
)
# port extensions
HONDA_CLARITY = HondaNidecPlatformConfig(
[HondaCarDocs("Honda Clarity 2018-21", min_steer_speed=12. * CV.MPH_TO_MS)],
CarSpecs(mass=1834, wheelbase=2.75, centerToFrontRatio=0.4, steerRatio=16.5),
radar_dbc_dict('honda_clarity_hybrid_2018_can_generated'),
flags=HondaFlags.HAS_ALL_DOOR_STATES,
)
HONDA_NIDEC_ALT_PCM_ACCEL = CAR.with_flags(HondaFlags.NIDEC_ALT_PCM_ACCEL)
HONDA_NIDEC_ALT_SCM_MESSAGES = CAR.with_flags(HondaFlags.NIDEC_ALT_SCM_MESSAGES)
HONDA_BOSCH = CAR.with_flags(HondaFlags.BOSCH)
HONDA_BOSCH_RADARLESS = CAR.with_flags(HondaFlags.BOSCH_RADARLESS)
HONDA_BOSCH_CANFD = CAR.with_flags(HondaFlags.BOSCH_CANFD)
HONDA_BOSCH_ALT_RADAR = CAR.with_flags(HondaFlags.BOSCH_ALT_RADAR)
HONDA_BOSCH_TJA_CONTROL = CAR.with_flags(HondaFlags.BOSCH_TJA_CONTROL)
# Bosch harness family whose radar broadcasts the decodable 16-slot object scan
HONDA_RADAR_SCAN_CAPABLE = HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD - HONDA_BOSCH_ALT_RADAR
# scan decode stays off per platform until a real route capture has been validated
HONDA_RADAR_SCAN_VERIFIED = frozenset({CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G})
DBC = CAR.create_dbc_map()
STEER_THRESHOLD = {
# default is 1200, overrides go here
CAR.ACURA_RDX: 400,
CAR.HONDA_CRV_EU: 400,
CAR.HONDA_ACCORD_11G: 600,
CAR.HONDA_PILOT_4G: 600,
CAR.HONDA_PASSPORT_4G: 600,
CAR.ACURA_MDX_4G_MMR: 600,
CAR.HONDA_CRV_6G: 600,
CAR.HONDA_CITY_7G: 600,
CAR.HONDA_NBOX_2G: 600,
}
HONDA_ALT_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xF112)
HONDA_ALT_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(0xF112)
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
# Currently used to fingerprint
Request(
[StdQueries.UDS_VERSION_REQUEST],
[StdQueries.UDS_VERSION_RESPONSE],
bus=1,
),
# Data collection requests:
# Log manufacturer-specific identifier for current ECUs
Request(
[HONDA_ALT_VERSION_REQUEST],
[HONDA_ALT_VERSION_RESPONSE],
bus=1,
logging=True,
),
# Nidec PT bus
Request(
[StdQueries.UDS_VERSION_REQUEST],
[StdQueries.UDS_VERSION_RESPONSE],
bus=0,
),
# Bosch PT bus
Request(
[StdQueries.UDS_VERSION_REQUEST],
[StdQueries.UDS_VERSION_RESPONSE],
bus=1,
obd_multiplexing=False,
),
],
# We lose these ECUs without the comma power on these cars.
# Note that we still attempt to match with them when they are present
# This is or'd with (ALL_ECUS - ESSENTIAL_ECUS) from fw_versions.py
non_essential_ecus={
Ecu.eps: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_E, *HONDA_BOSCH_ALT_RADAR, *HONDA_BOSCH_RADARLESS, *HONDA_BOSCH_CANFD],
Ecu.vsa: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CRV_5G, CAR.HONDA_CRV_HYBRID,
CAR.HONDA_E, CAR.HONDA_INSIGHT, CAR.HONDA_NBOX_2G, *HONDA_BOSCH_ALT_RADAR, *HONDA_BOSCH_RADARLESS, *HONDA_BOSCH_CANFD],
},
extra_ecus=[
(Ecu.combinationMeter, 0x18da60f1, None),
(Ecu.programmedFuelInjection, 0x18da10f1, None),
# The only other ECU on PT bus accessible by camera on radarless Civic
# This is likely a manufacturer-specific sub-address implementation: the camera responds to this and 0x18dab0f1
# Unclear what the part number refers to: 8S103 is 'Camera Set Mono', while 36160 is 'Camera Monocular - Honda'
# TODO: add query back, camera does not support querying both in parallel and 0x18dab0f1 often fails to respond
# (Ecu.unknown, 0x18DAB3F1, None),
],
)

0
artifacts/package_runtime/iqdbc/car/hyundai/__init__.py Normal file
View File

788
artifacts/package_runtime/iqdbc/car/hyundai/carcontroller.py Normal file
View File

@@ -0,0 +1,788 @@
import numpy as np
from iqdbc.can import CANPacker
from iqdbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
from iqdbc.car.lateral import apply_driver_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.hyundai import hyundaicanfd, hyundaican
from iqdbc.car.hyundai.carstate import CarState
from iqdbc.car.hyundai.hyundaicanfd import CanBus
from iqdbc.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CAR, CAN_GEARS, HyundaiExtFlags
from iqdbc.car.interfaces import CarControllerBase
from iqdbc.car.vehicle_model import VehicleModel
VisualAlert = structs.CarControl.HUDControl.VisualAlert
LongCtrlState = structs.CarControl.Actuators.LongControlState
from iqpilot.common.params import Params
# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
# All slightly below EPS thresholds to avoid fault
MAX_ANGLE = 85
MAX_ANGLE_FRAMES = 89
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
DRIVER_TORQUE_FILTER_TAU = 0.12
PRE_OVERRIDE_PREDICTION_TIME = 0.15
PRE_OVERRIDE_START_RATIO = 0.90
PRE_OVERRIDE_FULL_RATIO = 1.05
PRE_OVERRIDE_RAW_MIN_RATIO = 0.70
PRE_OVERRIDE_FILTERED_MIN_RATIO = 0.65
PRE_OVERRIDE_MIN_RATE_RATIO = 0.50
PRE_OVERRIDE_CONFIRM_FRAMES = 2
PRE_OVERRIDE_MAX_TORQUE_DELTA = -10.0
LOW_SPEED_ANGLE_RATE_RAMP_SPEED = 15.0 * CV.KPH_TO_MS
MID_SPEED_ANGLE_RATE_LIMIT_SPEED = 40.0 * CV.KPH_TO_MS
vibrate_intervals = [
(0.0, 0.5),
(1.0, 1.5),
#(2.5, 3.0),
#(3.5, 4.0),
(5.0, 5.5),
(6.0, 6.5),
(7.5, 8.0),
]
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
# initialize to no line visible
# TODO: this is not accurate for all cars
sys_state = 1
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
sys_state = 3 if enabled or sys_warning else 4
elif hud_control.leftLaneVisible:
sys_state = 5
elif hud_control.rightLaneVisible:
sys_state = 6
# initialize to no warnings
left_lane_warning = 0
right_lane_warning = 0
if hud_control.leftLaneDepart:
left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
if hud_control.rightLaneDepart:
right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
return sys_warning, sys_state, left_lane_warning, right_lane_warning
def rate_limit(x, x_last, lo, hi):
return float(np.clip(x, x_last + lo, x_last + hi))
def apply_steer_angle_limits_physics(desired_sw_deg: float,
last_sw_deg: float,
v_ego: float,
steering_sw_deg: float,
lat_active: bool,
wheelbase_m: float,
steer_ratio: float,
steer_sw_max_deg: float,
model_v2=None) -> float:
max_lat_accel = 8.5 # m/s^2
max_lat_jerk = 4.0 # m/s^3
y_std_1s = 0.1
if model_v2 is not None and len(model_v2.position.yStd) > 10:
model_y_std_1s = float(model_v2.position.yStd[10])
if np.isfinite(model_y_std_1s) and model_y_std_1s >= 0.0:
y_std_1s = model_y_std_1s
max_sw_rate_deg_per_tick = float(np.interp(y_std_1s, [0.1, 0.2, 0.4], [2.0, 1.5, 0.8]))
v = max(float(v_ego), 1.0)
target_sw = float(np.clip(desired_sw_deg, -steer_sw_max_deg, steer_sw_max_deg))
if v_ego < MID_SPEED_ANGLE_RATE_LIMIT_SPEED:
# Keep low/mid-speed angle commands quieter without reducing LKAS_ANGLE_MAX_TORQUE.
# Allow the angle, but slow the arrival: 0~15 kph ramps 0.8->1.1 deg/tick,
# then 15~40 kph tapers 1.1->0.8 deg/tick. Above 40 kph, physics limits take over.
low_mid_speed_cap = float(np.interp(v_ego,
[0.0, LOW_SPEED_ANGLE_RATE_RAMP_SPEED, MID_SPEED_ANGLE_RATE_LIMIT_SPEED],
[0.8, 1.1, 0.8]))
max_sw_rate_deg_per_tick = min(max_sw_rate_deg_per_tick, low_mid_speed_cap)
target_rw = target_sw / steer_ratio
last_rw = float(last_sw_deg) / steer_ratio
# --- accel limit ---
rw_max_rad = np.arctan((max_lat_accel * wheelbase_m) / (v * v))
rw_max = float(np.degrees(rw_max_rad))
# --- jerk -> rate limit ---
sec2 = 1.2
max_drw_dt = (max_lat_jerk * wheelbase_m) / (v * v * sec2) # rad/s
max_drw_per_tick = max_drw_dt * DT_CTRL # rad/tick
max_drw_per_tick_deg = float(np.degrees(max_drw_per_tick))
err = abs(target_sw - last_sw_deg)
if err > 20.0:
max_sw_rate_deg_per_tick = min(max_sw_rate_deg_per_tick, 1.0)
max_drw_per_tick_deg = min(
max_drw_per_tick_deg,
max_sw_rate_deg_per_tick / steer_ratio
)
# --- rate limit ---
cmd_rw = rate_limit(target_rw, last_rw, -max_drw_per_tick_deg, max_drw_per_tick_deg)
# --- accel clip ---
cmd_rw = float(np.clip(cmd_rw, -rw_max, rw_max))
if not lat_active:
cmd_rw = float(steering_sw_deg) / steer_ratio
cmd_sw = cmd_rw * steer_ratio
return float(np.clip(cmd_sw, -steer_sw_max_deg, steer_sw_max_deg))
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_IQ=None):
super().__init__(dbc_names, CP, CP_IQ or structs.IQCarParams())
self.CAN = CanBus(CP)
self.params = CarControllerParams(CP)
self.packer = CANPacker(dbc_names[Bus.pt])
self.angle_limit_counter = 0
self.accel_last = 0
self.apply_torque_last = 0
self.car_fingerprint = CP.carFingerprint
self.last_button_frame = 0
self.hyundai_jerk = HyundaiJerk()
self.speedCameraHapticEndFrame = 0
self.hapticFeedbackWhenSpeedCamera = 0
self.max_angle_frames = MAX_ANGLE_FRAMES
self.blinking_signal = False # 1Hz
self.blinking_frame = int(1.0 / DT_CTRL)
self.soft_hold_mode = 2
self.activateCruise = 0
self.button_wait = 12
self.cruise_buttons_msg_values = None
self.cruise_buttons_msg_cnt = 0
self.button_spamming_count = 0
self.prev_clu_speed = 0
self.button_spam1 = 8
self.button_spam2 = 30
self.button_spam3 = 1
self.apply_angle_last = 0
self.lkas_max_torque = 0
self.angle_max_torque = 250
self.steering_pressed_prev = False
self.recovering_from_override = False
self.full_recovery_frames = 0
self.repeated_override_count = 0
self.override_latched = False
self.override_release_frames = 0
self.driver_torque_filtered = 0.0
self.driver_torque_filtered_prev = 0.0
self.pre_override_frames = 0
self.lkas11_active = False
self.canfd_debug = 0
self.MainMode_ACC_trigger = 0
self.LFA_trigger = 0
self.activeCarrot = 0
self.camera_scc_params = Params().get_int("HyundaiCameraSCC")
self.is_ldws_car = Params().get_bool("IsLdwsCar")
self.enable_corner_radar = 0
self.steerDeltaUpOrg = self.steerDeltaUp = self.steerDeltaUpLC = self.params.STEER_DELTA_UP
self.steerDeltaDownOrg = self.steerDeltaDown = self.steerDeltaDownLC = self.params.STEER_DELTA_DOWN
def update(self, CC, CC_IQ, CS, now_nanos):
if self.frame % 50 == 0:
params = Params()
self.max_angle_frames = params.get_int("MaxAngleFrames")
steerMax = params.get_int("CustomSteerMax")
steerDeltaUp = params.get_int("CustomSteerDeltaUp")
steerDeltaDown = params.get_int("CustomSteerDeltaDown")
steerDeltaUpLC = params.get_int("CustomSteerDeltaUpLC")
steerDeltaDownLC = params.get_int("CustomSteerDeltaDownLC")
if steerMax > 0:
self.params.STEER_MAX = steerMax
if steerDeltaUp > 0:
self.steerDeltaUp = steerDeltaUp
#self.params.ANGLE_TORQUE_UP_RATE = steerDeltaUp
else:
self.steerDeltaUp = self.steerDeltaUpOrg
if steerDeltaDown > 0:
self.steerDeltaDown = steerDeltaDown
#self.params.ANGLE_TORQUE_DOWN_RATE = steerDeltaDown
else:
self.steerDeltaDown = self.steerDeltaDownOrg
if steerDeltaUpLC > 0:
self.steerDeltaUpLC = steerDeltaUpLC
else:
self.steerDeltaUpLC = self.steerDeltaUp
if steerDeltaDownLC > 0:
self.steerDeltaDownLC = steerDeltaDownLC
else:
self.steerDeltaDownLC = self.steerDeltaDown
self.soft_hold_mode = 1 if params.get_int("AutoCruiseControl") > 1 else 2
self.hapticFeedbackWhenSpeedCamera = int(params.get_int("HapticFeedbackWhenSpeedCamera"))
self.button_spam1 = params.get_int("CruiseButtonTest1")
self.button_spam2 = params.get_int("CruiseButtonTest2")
self.button_spam3 = params.get_int("CruiseButtonTest3")
self.speed_from_pcm = params.get_int("SpeedFromPCM")
self.canfd_debug = params.get_int("CanfdDebug")
self.camera_scc_params = params.get_int("HyundaiCameraSCC")
self.enable_corner_radar = params.get_int("EnableCornerRadar")
actuators = CC.actuators
hud_control = CC.hudControl
if hud_control.modelDesire in [3,4]:
self.params.STEER_DELTA_UP = self.steerDeltaUpLC
self.params.STEER_DELTA_DOWN = self.steerDeltaDownLC
else:
self.params.STEER_DELTA_UP = self.steerDeltaUp
self.params.STEER_DELTA_DOWN = self.steerDeltaDown
angle_control = self.CP.flags & HyundaiFlags.ANGLE_CONTROL
# steering torque
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.params)
# >90 degree steering fault prevention
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
self.angle_limit_counter, self.max_angle_frames,
MAX_ANGLE_CONSECUTIVE_FRAMES)
#apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw,
# CS.out.steeringAngleDeg, CC.latActive, self.params.ANGLE_LIMITS)
apply_angle = apply_steer_angle_limits_physics(
actuators.steeringAngleDeg,
self.apply_angle_last,
CS.out.vEgoRaw,
CS.out.steeringAngleDeg,
CC.latActive,
self.CP.wheelbase,
self.CP.steerRatio,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
CS.modelV2,
)
if angle_control:
apply_steer_req = CC.latActive
angle_torque_cap = self.angle_max_torque
steering_pressed_rising = CS.out.steeringPressed and not self.steering_pressed_prev
if steering_pressed_rising:
if 0 < self.full_recovery_frames < int(5.0 / DT_CTRL):
self.repeated_override_count = min(self.repeated_override_count + 1, 3)
self.full_recovery_frames = 0
self.recovering_from_override = True
torque_threshold = max(self.params.STEER_THRESHOLD, 1.0)
# Filter signed torque so alternating sensor noise cancels out before its
# magnitude is used for pre-override prediction.
driver_torque = float(CS.out.steeringTorque)
driver_torque_abs = abs(driver_torque)
if not CC.latActive:
self.driver_torque_filtered = driver_torque
self.driver_torque_filtered_prev = driver_torque
self.pre_override_frames = 0
else:
torque_filter_alpha = DT_CTRL / (DRIVER_TORQUE_FILTER_TAU + DT_CTRL)
self.driver_torque_filtered_prev = self.driver_torque_filtered
self.driver_torque_filtered += torque_filter_alpha * (driver_torque - self.driver_torque_filtered)
driver_torque_filtered_abs = abs(self.driver_torque_filtered)
driver_torque_filtered_prev_abs = abs(self.driver_torque_filtered_prev)
driver_torque_rate = max(0.0, (driver_torque_filtered_abs - driver_torque_filtered_prev_abs) / DT_CTRL)
torque_ratio = driver_torque_filtered_abs / torque_threshold
raw_torque_ratio = driver_torque_abs / torque_threshold
predicted_torque_ratio = (
driver_torque_filtered_abs + driver_torque_rate * PRE_OVERRIDE_PREDICTION_TIME
) / torque_threshold
pre_override_candidate = (
CC.latActive and
not CS.out.steeringPressed and
raw_torque_ratio > PRE_OVERRIDE_RAW_MIN_RATIO and
torque_ratio > PRE_OVERRIDE_FILTERED_MIN_RATIO and
predicted_torque_ratio > PRE_OVERRIDE_START_RATIO and
driver_torque_rate > torque_threshold * PRE_OVERRIDE_MIN_RATE_RATIO
)
self.pre_override_frames = self.pre_override_frames + 1 if pre_override_candidate else 0
pre_override_yield = 0.0
if self.pre_override_frames >= PRE_OVERRIDE_CONFIRM_FRAMES:
pre_override_yield = float(np.interp(
predicted_torque_ratio,
[PRE_OVERRIDE_START_RATIO, PRE_OVERRIDE_FULL_RATIO],
[0.0, 1.0],
))
recovery_allowed = False
if CS.out.steeringPressed:
# Start yielding immediately when driver override is confirmed.
self.override_latched = True
self.override_release_frames = 0
torque_delta = -20.0
elif pre_override_yield > 0.0:
# Start handing off gently before steeringPressed flips to avoid a sharp torque drop.
torque_delta = PRE_OVERRIDE_MAX_TORQUE_DELTA * pre_override_yield
elif self.lkas_max_torque >= self.angle_max_torque:
# Once fully recovered, hold full authority until the next driver override.
torque_delta = 0.0
elif self.override_latched:
# Hold reduced authority until driver torque stays below 60% for 0.2 seconds.
self.override_release_frames = self.override_release_frames + 1 if torque_ratio < 0.6 else 0
if self.override_release_frames >= int(0.2 / DT_CTRL):
self.override_latched = False
self.override_release_frames = 0
recovery_allowed = True
else:
torque_delta = 0.0
else:
recovery_allowed = True
if recovery_allowed:
# Use one-second model uncertainty to set the base torque recovery time.
# Missing or invalid model data falls back to a moderate 1.5-second recovery.
y_std_1s = 0.2
if CS.modelV2 is not None and len(CS.modelV2.position.yStd) > 10:
model_y_std_1s = float(CS.modelV2.position.yStd[10])
if np.isfinite(model_y_std_1s) and model_y_std_1s >= 0.0:
y_std_1s = model_y_std_1s
recovery_time = float(np.interp(y_std_1s, [0.1, 0.2, 0.3, 0.4], [0.5, 0.8, 1.5, 3.0]))
recovery_time = max(recovery_time, float(np.interp(
self.repeated_override_count,
[0, 1, 2, 3],
[0.1, 1.0, 2.0, 3.0],
)))
base_rate_up = (self.angle_max_torque - self.params.ANGLE_MIN_TORQUE) * DT_CTRL / recovery_time
# During recovery, taper the rate to zero. Only steeringPressed can reduce authority.
torque_delta = base_rate_up * float(np.interp(torque_ratio, [0.6, 0.8], [1.0, 0.0]))
self.lkas_max_torque = float(np.clip(self.lkas_max_torque + torque_delta,
self.params.ANGLE_MIN_TORQUE, angle_torque_cap))
if not CS.out.steeringPressed and self.recovering_from_override and self.lkas_max_torque >= self.angle_max_torque:
self.recovering_from_override = False
self.full_recovery_frames = 1
elif not CS.out.steeringPressed and self.full_recovery_frames > 0:
self.full_recovery_frames += 1
if self.full_recovery_frames >= int(5.0 / DT_CTRL):
self.full_recovery_frames = 0
self.repeated_override_count = 0
if not CC.latActive:
apply_torque = 0
self.lkas_max_torque = 0
self.recovering_from_override = False
self.full_recovery_frames = 0
self.repeated_override_count = 0
self.override_latched = False
self.override_release_frames = 0
self.driver_torque_filtered = 0.0
self.driver_torque_filtered_prev = 0.0
self.pre_override_frames = 0
self.steering_pressed_prev = CS.out.steeringPressed if CC.latActive else False
self.apply_angle_last = apply_angle
# Hold torque with induced temporary fault when cutting the actuation bit
torque_fault = CC.latActive and not apply_steer_req
self.apply_torque_last = apply_torque
# accel + longitudinal
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
active_speed_decel = hud_control.activeCarrot == 3 and self.activeCarrot != 3 # 3: Speed Decel
self.activeCarrot = hud_control.activeCarrot
if active_speed_decel and self.speedCameraHapticEndFrame < 0: # 과속카메라 감속시작
self.speedCameraHapticEndFrame = self.frame + (8.0 / DT_CTRL) #8초간 켜줌.
elif not active_speed_decel:
self.speedCameraHapticEndFrame = -1
if 0 <= self.speedCameraHapticEndFrame - self.frame < int(8.0 / DT_CTRL) and self.hapticFeedbackWhenSpeedCamera > 0:
t = (self.frame - (self.speedCameraHapticEndFrame - int(8.0 / DT_CTRL))) * DT_CTRL
for start, end in vibrate_intervals:
if start <= t < end:
left_lane_warning = right_lane_warning = self.hapticFeedbackWhenSpeedCamera
break
if self.frame >= self.speedCameraHapticEndFrame:
self.speedCameraHapticEndFrame = -1
if self.frame % self.blinking_frame == 0:
self.blinking_signal = True
elif self.frame % self.blinking_frame == self.blinking_frame / 2:
self.blinking_signal = False
can_sends = []
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, self.CAN.ECAN if self.CP.flags & HyundaiFlags.CANFD else 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, self.CAN.ECAN
can_sends.append(make_tester_present_msg(addr, bus, suppress_response=True))
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append(make_tester_present_msg(0x7b1, self.CAN.ECAN, suppress_response=True))
camera_scc = self.CP.flags & HyundaiFlags.CAMERA_SCC
# CAN-FD platforms
if self.CP.flags & HyundaiFlags.CANFD:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(
self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque,
apply_angle, self.lkas_max_torque, angle_control,
))
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
if self.frame % 5 == 0 and hda2 and not camera_scc:
can_sends.extend(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS))
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long or camera_scc):
can_sends.extend(hyundaicanfd.create_lfahda_cluster(self.packer, CS, self.CAN, CC.longActive, CC.latActive))
if not camera_scc:
can_sends.extend(hyundaicanfd.create_lfa_icon_non_camera_scc(self.packer, CS, self.CAN, CC))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
if self.camera_scc_params in [2, 3]:
self.canfd_toggle_adas(CC, CS)
if self.CP.openpilotLongitudinalControl:
self.hyundai_jerk.make_jerk(self.CP, CS, accel, actuators, hud_control)
self.hyundai_jerk.check_carrot_cruise(CC, CS, hud_control, stopping, accel, actuators.aTarget)
if True: #not camera_scc:
can_sends.extend(hyundaicanfd.create_ccnc_messages(self.CP, self.packer, self.CAN, self.frame, CC, CS, hud_control, apply_angle, left_lane_warning, right_lane_warning, self.enable_corner_radar, stopping, self.canfd_debug))
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.CP, self.packer, self.CAN, self.frame))
else:
can_sends.extend(hyundaicanfd.create_fca_warning_light(self.CP, self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
if self.CP.flags & HyundaiFlags.CAMERA_SCC.value:
msg = hyundaicanfd.create_acc_control_scc2(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, hud_control, self.hyundai_jerk, CS)
if msg is not None:
can_sends.append(msg)
can_sends.extend(hyundaicanfd.create_tcs_messages(self.packer, self.CAN, CS)) # for sorento SCC radar...
else:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, hud_control, self.hyundai_jerk.jerk_u, self.hyundai_jerk.jerk_l, CS))
self.accel_last = accel
else:
# button presses
if self.camera_scc_params == 3: # camera scc but stock long
send_button = self.make_spam_button(CC, CS)
can_sends.extend(hyundaicanfd.forward_button_message(self.packer, self.CAN, self.frame, CS, send_button, self.MainMode_ACC_trigger, self.LFA_trigger))
else:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
else:
if CS.lkas11 is not None:
if self.lkas11_active:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_torque, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning, self.is_ldws_car))
self.lkas11_active = True
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
if self.CP.carFingerprint in CAN_GEARS["send_mdps12"] and CS.mdps12 is not None: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(hyundaican.create_mdps12(self.packer, self.frame, CS.mdps12))
casper_ev = self.CP.carFingerprint == CAR.HYUNDAI_CASPER_EV
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
self.hyundai_jerk.make_jerk(self.CP, CS, accel, actuators, hud_control)
self.hyundai_jerk.check_carrot_cruise(CC, CS, hud_control, stopping, accel, actuators.aTarget)
#jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
if camera_scc:
can_sends.extend(hyundaican.create_acc_commands_scc(self.packer, CC.enabled, accel, self.hyundai_jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, casper_ev, CS, self.soft_hold_mode))
else:
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, self.hyundai_jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, self.CP, CS, self.soft_hold_mode))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC, self.blinking_signal))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
if camera_scc:
if CS.scc13 is not None:
if casper_ev:
#can_sends.append(hyundaican.create_acc_opt_copy(CS, self.packer))
pass
pass
else:
can_sends.extend(hyundaican.create_acc_opt(self.packer, self.CP))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl and not camera_scc:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
new_actuators = actuators.as_builder()
new_actuators.torque = apply_torque / self.params.STEER_MAX
# torqueOutputCan reflects the steering authority value actually sent over CAN.
# Torque-control platforms send the signed torque command, while angle-control
# platforms send LKAS_ANGLE_MAX_TORQUE alongside the requested angle.
new_actuators.torqueOutputCan = self.lkas_max_torque if angle_control else apply_torque
new_actuators.steeringAngleDeg = float(apply_angle)
new_actuators.accel = accel
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: structs.CarControl, CS: CarState, use_clu11: bool):
can_sends = []
if CS.out.brakePressed or CS.out.brakeHoldActive:
return can_sends
if use_clu11:
if CS.clu11 is None:
return can_sends
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
elif False: #CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP)] * 25)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
if self.last_button_frame != self.frame:
send_button = self.make_spam_button(CC, CS)
if send_button > 0:
can_sends.append(hyundaican.create_clu11_button(self.packer, self.frame, CS.clu11, send_button, self.CP))
else:
# carrot.. 왜 alt_cruise_button는 값이 리스트일까?, 그리고 왜? 빈데이터가 들어오는것일까?
if CS.cruise_buttons_msg is not None and self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
try:
cruise_buttons_msg_values = {key: value[0] for key, value in CS.cruise_buttons_msg.items()}
except: # IndexError:
#print("IndexError....")
cruise_buttons_msg_values = None
self.cruise_buttons_msg_cnt += 1
if cruise_buttons_msg_values is not None:
self.cruise_buttons_msg_values = cruise_buttons_msg_values
self.cruise_buttons_msg_cnt = 0
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# cruise cancel
if CC.cruiseControl.cancel:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
print("cruiseControl.cancel222222")
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
#can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.scc_control))
if self.cruise_buttons_msg_values is not None:
can_sends.append(hyundaicanfd.alt_cruise_buttons(self.packer, self.CP, self.CAN, Buttons.CANCEL, self.cruise_buttons_msg_values, self.cruise_buttons_msg_cnt))
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif False: #CC.cruiseControl.resume:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# TODO: resume for alt button cars
pass
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
## button 스패밍을 안했을때...
if self.last_button_frame != self.frame:
dat = self.canfd_speed_control_pcm(CC, CS, self.cruise_buttons_msg_values)
if dat is not None:
for _ in range(self.button_spam3):
can_sends.append(dat)
self.cruise_buttons_msg_cnt += 1
return can_sends
def canfd_toggle_adas(self, CC, CS):
trigger_min = -200
trigger_start = 6
self.MainMode_ACC_trigger = max(trigger_min, self.MainMode_ACC_trigger - 1)
self.LFA_trigger = max(trigger_min, self.LFA_trigger - 1)
if self.MainMode_ACC_trigger == trigger_min and self.LFA_trigger == trigger_min:
if CC.enabled and not CS.MainMode_ACC and CS.out.vEgo > 3.:
self.MainMode_ACC_trigger = trigger_start
elif CC.latActive and CS.LFA_ICON == 0:
self.LFA_trigger = trigger_start
def canfd_speed_control_pcm(self, CC, CS, cruise_buttons_msg_values):
alt_buttons = True if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else False
if alt_buttons and cruise_buttons_msg_values is None:
return None
send_button = self.make_spam_button(CC, CS)
if send_button > 0:
if alt_buttons:
return hyundaicanfd.alt_cruise_buttons(self.packer, self.CP, self.CAN, send_button, cruise_buttons_msg_values, self.cruise_buttons_msg_cnt)
else:
return hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, send_button)
return None
def make_spam_button(self, CC, CS):
hud_control = CC.hudControl
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
target = int(set_speed_in_units+0.5)
current = int(CS.out.cruiseState.speed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) + 0.5)
v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH
send_button = 0
activate_cruise = False
if CC.enabled:
if not CS.out.cruiseState.enabled:
if (hud_control.leadVisible or v_ego_kph > 10.0) and self.activateCruise == 0:
send_button = Buttons.RES_ACCEL
self.activateCruise = 1
activate_cruise = True
elif CC.cruiseControl.resume:
send_button = Buttons.RES_ACCEL
elif target < current and current>= 31 and self.speed_from_pcm != 1:
send_button = Buttons.SET_DECEL
elif target > current and current < 160 and self.speed_from_pcm != 1:
send_button = Buttons.RES_ACCEL
elif CS.out.activateCruise: #CC.cruiseControl.activate:
if (hud_control.leadVisible or v_ego_kph > 10.0) and self.activateCruise == 0:
self.activateCruise = 1
send_button = Buttons.RES_ACCEL
activate_cruise = True
if CS.out.brakePressed or CS.out.gasPressed:
self.activateCruise = 0
if send_button == 0:
self.button_spamming_count = 0
self.prev_clu_speed = current
return 0
speed_diff = self.prev_clu_speed - current
spamming_max = self.button_spam1
if CS.cruise_buttons[-1] != Buttons.NONE:
self.last_button_frame = self.frame
self.button_wait = self.button_spam2
self.button_spamming_count = 0
elif abs(self.button_spamming_count) >= spamming_max or abs(speed_diff) > 0:
self.last_button_frame = self.frame
self.button_wait = self.button_spam2 if abs(self.button_spamming_count) >= spamming_max else 7
self.button_spamming_count = 0
self.prev_clu_speed = current
send_button_allowed = (self.frame - self.last_button_frame) > self.button_wait
#CC.debugTextCC = "{} speed_diff={:.1f},{:.0f}/{:.0f}, button={}, button_wait={}, count={}".format(
# send_button_allowed, speed_diff, target, current, send_button, self.button_wait, self.button_spamming_count)
if send_button_allowed or activate_cruise or (CC.cruiseControl.resume and self.frame % 2 == 0):
self.button_spamming_count = self.button_spamming_count + 1 if send_button == Buttons.RES_ACCEL else self.button_spamming_count - 1
return send_button
else:
self.button_spamming_count = 0
return 0
from iqpilot.common.filter_simple import MyMovingAverage
class HyundaiJerk:
def __init__(self):
self.params = Params()
self.jerk = 0.0
self.jerk_u = self.jerk_l = 0.0
self.cb_upper = self.cb_lower = 0.0
self.jerk_u_min = 0.5
self.carrot_cruise = 1
self.carrot_cruise_accel = 0.0
def check_carrot_cruise(self, CC, CS, hud_control, stopping, accel, a_target):
carrot_cruise_decel = self.params.get_float("CarrotCruiseDecel")
carrot_cruise_atc_decel = self.params.get_float("CarrotCruiseAtcDecel")
if carrot_cruise_atc_decel >= 0 and 0 < hud_control.atcDistance < 500:
carrot_cruise_decel = max(carrot_cruise_decel, carrot_cruise_atc_decel)
self.carrot_cruise = 0
if CS.out.carrotCruise > 0 and not CC.cruiseControl.override:
if CS.softHoldActive == 0 and not stopping:
if CS.out.vEgo > 10/3.6:
if carrot_cruise_decel < 0:
if (a_target > -0.1 or accel > -0.1):
self.carrot_cruise = 1
self.carrot_cruise_accel = 0.0
else:
self.carrot_cruise = 2
carrot_cruise = min(accel, -carrot_cruise_decel * 0.01)
self.carrot_cruise_accel = max(carrot_cruise, self.carrot_cruise_accel - 1.0 * DT_CTRL) # 점진적으로 줄임.
if self.carrot_cruise == 0:
self.carrot_cruise_accel = CS.out.aEgo
def make_jerk(self, CP, CS, accel, actuators, hud_control):
if actuators.longControlState == LongCtrlState.stopping:
self.jerk = self.jerk_u_min / 2 - CS.out.aEgo
else:
jerk = actuators.jerk if actuators.longControlState == LongCtrlState.pid else 0.0
#a_error = actuators.aTarget - CS.out.aEgo
self.jerk = jerk# + a_error
jerk_max_l = 5.0
jerk_max_u = jerk_max_l
if actuators.longControlState == LongCtrlState.off:
self.jerk_u = jerk_max_u
self.jerk_l = jerk_max_l
self.cb_upper = self.cb_lower = 0.0
else:
if CP.flags & HyundaiFlags.CANFD:
# Keep deceleration authority after the MPC jerk settles to zero. Stock SCC raises the
# lower jerk limit with the raw acceleration request instead of relying on jerk alone.
jerk_l_base = 1.2
jerk_l_raw = np.clip(jerk_l_base + 2.0 * max(0.0, -accel - 2.8), jerk_l_base, jerk_max_l)
jerk_l_mpc = np.clip(-self.jerk * 4.0, jerk_l_base, jerk_max_l)
self.jerk_u = min(max(self.jerk_u_min, self.jerk * 2.0), jerk_max_u)
self.jerk_l = max(jerk_l_raw, jerk_l_mpc)
self.cb_upper = self.cb_lower = 0.0
else:
self.jerk_u = min(max(self.jerk_u_min, self.jerk * 2.0), jerk_max_u)
self.jerk_l = min(max(1.0, -self.jerk * 4.0), jerk_max_l)
self.cb_upper = np.clip(0.9 + accel * 0.2, 0, 1.2)
self.cb_lower = np.clip(0.8 + accel * 0.2, 0, 1.2)

839
artifacts/package_runtime/iqdbc/car/hyundai/carstate.py Normal file
View File

@@ -0,0 +1,839 @@
from collections import deque
import copy
import math
import numpy as np
import ast
from iqdbc.can import CANDefine, CANParser
from iqdbc.car import Bus, create_button_events, structs, DT_CTRL
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.hyundai.hyundaicanfd import CanBus
from iqdbc.car.hyundai.values import HyundaiFlags, CAR, DBC, Buttons, CarControllerParams, CAMERA_SCC_CAR, HyundaiExtFlags, \
EV_MODE_ACTIVE_VALUES, EV_MODE_STATUS_ADDR, EV_MODE_STATUS_DLC, EV_MODE_STATUS_MSG, \
EV_MODE_STATUS_SIGNAL
from iqdbc.car.interfaces import CarStateBase
from iqpilot.common.params import Params
from datetime import datetime
from zoneinfo import ZoneInfo
ButtonType = structs.CarState.ButtonEvent.Type
PREV_BUTTON_SAMPLES = 8
CLUSTER_SAMPLE_RATE = 20 # frames
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise,
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel, Buttons.LFA_BUTTON: ButtonType.lfaButton}
GearShifter = structs.CarState.GearShifter
READY_COUNT_OK = 200
TRAILER_DISCONNECT_GRACE_FRAMES = int(5.0 / DT_CTRL)
EV_MODE_STATUS_TIMEOUT_NS = 500_000_000
def _get_ev_mode_state(cp: CANParser) -> tuple[bool, bool]:
timestamps = cp.ts_nanos.get(EV_MODE_STATUS_MSG)
if timestamps is None:
return False, False
timestamp = timestamps.get(EV_MODE_STATUS_SIGNAL, 0)
dat = cp.dat.get(EV_MODE_STATUS_ADDR, b"")
# The update timestamp advances even when the whole ECAN bus is silent.
# last_nonempty_nanos would leave the final decoded state valid forever.
age = cp._last_update_nanos - timestamp
valid = timestamp > 0 and len(dat) == EV_MODE_STATUS_DLC and not cp.bus_timeout and 0 <= age <= EV_MODE_STATUS_TIMEOUT_NS
active = valid and int(cp.vl[EV_MODE_STATUS_MSG][EV_MODE_STATUS_SIGNAL]) in EV_MODE_ACTIVE_VALUES
return active, valid
NUMERIC_TO_TZ = {
840: "America/New_York", # 미국 (US) → 동부 시간대
124: "America/Toronto", # 캐나다 (CA) → 동부 시간대
250: "Europe/Paris", # 프랑스 (FR)
276: "Europe/Berlin", # 독일 (DE)
826: "Europe/London", # 영국 (GB)
392: "Asia/Tokyo", # 일본 (JP)
156: "Asia/Shanghai", # 중국 (CN)
410: "Asia/Seoul", # 한국 (KR)
36: "Australia/Sydney", # 호주 (AU)
356: "Asia/Kolkata", # 인도 (IN)
}
class CarState(CarStateBase):
def __init__(self, CP, CP_IQ=None):
super().__init__(CP, CP_IQ or structs.IQCarParams())
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.cruise_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.main_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.gear_msg_canfd = "GEAR" if CP.extFlags & HyundaiExtFlags.CANFD_GEARS_69 else \
"ACCELERATOR" if CP.flags & HyundaiFlags.EV else \
"GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
"GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_SHIFTER"
self.use_accelerator = self.gear_msg_canfd == "ACCELERATOR"
if CP.flags & HyundaiFlags.CANFD:
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
elif CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
self.shifter_values = can_define.dv["ELECT_GEAR"]["Elect_Gear_Shifter"]
elif self.CP.flags & HyundaiFlags.CLUSTER_GEARS:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.flags & HyundaiFlags.TCU_GEARS:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
elif CP.flags & HyundaiFlags.FCEV:
self.shifter_values = can_define.dv["EMS20"]["HYDROGEN_GEAR_SHIFTER"]
else:
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.accelerator_msg_canfd = "ACCELERATOR" if CP.flags & HyundaiFlags.EV else \
"ACCELERATOR_ALT" if CP.flags & HyundaiFlags.HYBRID else \
"ACCELERATOR_BRAKE_ALT"
self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \
"CRUISE_BUTTONS"
self.is_metric = False
self.buttons_counter = 0
# for generic CAN parsing
self.fca11 = None
self.scc11 = None
self.scc12 = None
self.scc13 = None
self.scc14 = None
self.lkas11 = None
self.clu11 = None
# for CANFD parsing
self.scc_control = None
self.lfa = None
self.lfa_alt = None
self.lfahda_cluster = None
self.adrv_0x161 = None
self.adrv_0x200 = None
self.adrv_0x1ea = None
self.adrv_0x160 = None
self.ccnc_0x162 = None
self.hda_info_4a3 = None
self.tcs = None
self.mdps = None
self.steer_touch_2af = None
self.cruise_buttons_msg = None
self.cam_0x362 = None
self.cam_0x2a4 = None
self.manual_speed_limit_assist = None
self.accelerator = None
self.blinkers = None
self.blinkers_alt = None
self.doors_seatbelts = None
self.cruise_buttons_alt2 = None
# On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
self.cluster_speed = 0
self.cluster_speed_counter = CLUSTER_SAMPLE_RATE
self.params = CarControllerParams(CP)
self.op_params = Params()
self.main_enabled = True if self.op_params.get_int("AutoEngage") == 2 else False
self.gear_shifter = GearShifter.drive # Gear_init for Nexo ?? unknown 21.02.23.LSW
self.totalDistance = 0.0
self.speedLimitDistance = 0
self.pcmCruiseGap = 0
self.cruise_buttons_alt = True if self.CP.carFingerprint in (CAR.HYUNDAI_CASPER, CAR.HYUNDAI_CASPER_EV) else False
self.MainMode_ACC = False
self.ACCMode = 0
self.LFA_ICON = 0
self.paddle_button_prev = 0
self.lf_distance = 0
self.rf_distance = 0
self.lr_distance = 0
self.rr_distance = 0
#self.lf_lateral = 0
#self.rf_lateral = 0
fingerprints_str = Params().get("FingerPrints")
try:
fingerprints = ast.literal_eval(fingerprints_str) if fingerprints_str else {i: {} for i in range(8)}
except (SyntaxError, ValueError):
fingerprints = {i: {} for i in range(8)}
#print("fingerprints =", fingerprints)
ecu_disabled = False
if self.CP.openpilotLongitudinalControl and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC):
ecu_disabled = True
self.HAS_LFA_BUTTON = True if 913 in fingerprints[0] else False
self.CRUISE_BUTTON_ALT = True if 1007 in fingerprints[0] else False
cam_bus = CanBus(CP).CAM
pt_bus = CanBus(CP).ECAN
alt_bus = CanBus(CP).ACAN
self.GEAR = True if 69 in fingerprints[pt_bus] else False
self.GEAR_ALT = True if 64 in fingerprints[pt_bus] else False
self.TPMS = True if 0x3a0 in fingerprints[pt_bus] else False
self.LOCAL_TIME = True if 1264 in fingerprints[pt_bus] else False
self.cp_bsm = None
self.time_zone = "UTC"
self.cp = None
self.cp_cam = None
self.cp_alt = None
self.controls_ready_count = 0
# trailer detection
self.trailer_connected = False
self.trailer_timeout_cnt = 0
self.trailer_connected_prev = False
self.trailer_status = None
def monitor_fingerprint(self, can_parsers, canfd):
if self.controls_ready_count <= READY_COUNT_OK:
if Params().get_bool("ControlsReady"):
self.controls_ready_count += 1
self.cp = can_parsers[Bus.pt]
self.cp_cam = can_parsers[Bus.cam]
self.cp_alt = can_parsers[Bus.alt] if Bus.alt in can_parsers else None
def add_if_seen(parser, name, ignore_counter = False):
msg = parser.dbc.name_to_msg.get(name)
if not msg:
print(f"{name} not in DBC")
return
if msg.address not in parser.seen_addresses:
return
if msg.address in parser.addresses:
return
parser._add_message(name, ignore_counter = ignore_counter) # ← 이름으로 등록
def add_and_cache(parser, name: str, attr: str, ignore_counter: bool = False):
add_if_seen(parser, name, ignore_counter)
if name in parser.vl: # 등록 성공했을 때만
setattr(self, attr, parser.vl[name])
return True
return False
if self.controls_ready_count == 50:
self.cp.controls_ready = self.cp_cam.controls_ready = True
if self.cp_alt is not None:
self.cp_alt.controls_ready = True
elif self.controls_ready_count == 100:
self.cp.enable_capture = self.cp_cam.enable_capture = False
if self.cp_alt is not None:
self.cp_alt.enable_capture = False
elif self.controls_ready_count == 101:
print("cp_cam.seen_addresses =", self.cp_cam.seen_addresses)
elif self.controls_ready_count == 102:
print("cp.seen_addresses =", self.cp.seen_addresses)
elif self.controls_ready_count == 103:
if self.cp_alt is not None:
print("cp_alt.seen_addresses =", self.cp_alt.seen_addresses)
else:
print("cp_alt.seen_addresses = None")
if not canfd:
if self.controls_ready_count == 104:
cp_cruise = self.cp_cam if self.CP.flags & HyundaiFlags.CAMERA_SCC else self.cp
add_and_cache(cp_cruise, "FCA11", "fca11")
add_and_cache(self.cp_cam, "LKAS11", "lkas11")
add_and_cache(self.cp, "CLU11", "clu11")
elif self.controls_ready_count == 105:
cp_cruise = self.cp_cam if self.CP.flags & HyundaiFlags.CAMERA_SCC else self.cp
scc_messages_expected = not self.CP.openpilotLongitudinalControl or self.CP.flags & HyundaiFlags.CAMERA_SCC
if scc_messages_expected:
add_and_cache(cp_cruise, "SCC11", "scc11")
add_and_cache(cp_cruise, "SCC12", "scc12")
add_and_cache(cp_cruise, "SCC13", "scc13")
add_and_cache(cp_cruise, "SCC14", "scc14")
else: # canfd
if self.controls_ready_count == 120:
cp_cruise = self.cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else self.cp
add_and_cache(cp_cruise, "SCC_CONTROL", "scc_control")
elif self.controls_ready_count == 121:
add_and_cache(self.cp, "TCS", "tcs")
add_and_cache(self.cp, "MDPS", "mdps")
add_and_cache(self.cp_cam, "LFA", "lfa")
add_and_cache(self.cp_cam, "LFA_ALT", "lfa_alt")
add_and_cache(self.cp_cam, "LFAHDA_CLUSTER", "lfahda_cluster")
elif self.controls_ready_count == 122:
add_and_cache(self.cp_cam, "ADRV_0x161", "adrv_0x161")
add_and_cache(self.cp_cam, "ADRV_0x200", "adrv_0x200")
add_and_cache(self.cp_cam, "ADRV_0x1ea", "adrv_0x1ea")
add_and_cache(self.cp_cam, "ADRV_0x160", "adrv_0x160")
add_and_cache(self.cp_cam, "CCNC_0x162", "ccnc_0x162")
elif self.controls_ready_count == 123:
add_and_cache(self.cp, "HDA_INFO_4A3", "hda_info_4a3")
add_and_cache(self.cp, "STEER_TOUCH_2AF", "steer_touch_2af")
elif self.controls_ready_count == 124:
add_and_cache(self.cp, self.cruise_btns_msg_canfd, "cruise_buttons_msg")
if not add_and_cache(self.cp_cam, "CAM_0x362", "cam_0x362") and self.cp_alt is not None:
add_and_cache(self.cp_alt, "CAM_0x362", "cam_0x362")
if not add_and_cache(self.cp_alt, "CAM_0x2a4", "cam_0x2a4", ignore_counter=True) and self.cp_cam is not None:
add_and_cache(self.cp_cam, "CAM_0x2a4", "cam_0x2a4", ignore_counter=True)
elif self.controls_ready_count == 125:
add_and_cache(self.cp, "MANUAL_SPEED_LIMIT_ASSIST", "manual_speed_limit_assist", ignore_counter = True)
if self.gear_msg_canfd == "ACCELERATOR":
add_and_cache(self.cp, "ACCELERATOR", "accelerator", ignore_counter = True)
add_and_cache(self.cp, "BLINKERS", "blinkers")
add_and_cache(self.cp, "BLINKERS_ALT", "blinkers_alt")
add_and_cache(self.cp, "DOORS_SEATBELTS", "doors_seatbelts")
elif self.controls_ready_count == 126:
add_and_cache(self.cp, "CRUISE_BUTTONS_ALT2", "cruise_buttons_alt2", ignore_counter = True)
add_and_cache(self.cp, "TRAILER_STATUS", "trailer_status", ignore_counter = True)
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
self.monitor_fingerprint(can_parsers, self.CP.flags & HyundaiFlags.CANFD)
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
cp_alt = can_parsers[Bus.alt] if Bus.alt in can_parsers else None
if self.CP.flags & HyundaiFlags.CANFD:
return self.update_canfd(can_parsers), self.out_iq
ret = structs.CarState()
cp_cruise = cp_cam if self.CP.flags & HyundaiFlags.CAMERA_SCC else cp
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
if cp.ts_nanos["EMS21"]["SCR_UREA_LEVEL"] > 0:
ret.ureaGauge = float(np.clip(cp.vl["EMS21"]["SCR_UREA_LEVEL"] / 100.0, 0.0, 1.0))
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
self.cluster_speed_counter += 1
if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
self.cluster_speed_counter = 0
# Mimic how dash converts to imperial.
# Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric
# TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this
if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,):
self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
#ret.vEgoCluster = self.cluster_speed * speed_conv
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
# cruise state
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.available = self.main_enabled and self.controls_ready_count >= READY_COUNT_OK #cp.vl["TCS13"]["ACCEnable"] == 0
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
ret.cruiseState.nonAdaptive = False
elif not self.CP.flags & HyundaiFlags.CC_ONLY_CAR:
self.main_enabled = ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 2. # Shows 'Cruise Control' on dash
ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
ret.pcmCruiseGap = cp_cruise.vl["SCC11"]["TauGapSet"]
# TODO: Find brake pressure
ret.brake = 0
if not self.CP.flags & HyundaiFlags.CC_ONLY_CAR:
ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2 # 2 includes regen braking by user on HEV/EV
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
ret.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1
ret.espActive = cp.vl["TCS11"]["ABS_ACT"] == 1
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
ret.brakeLights = bool(cp.vl["TCS13"]["BrakeLight"] or ret.brakePressed)
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV | HyundaiFlags.FCEV):
if self.CP.flags & HyundaiFlags.FCEV:
ret.gas = cp.vl["FCEV_ACCELERATOR"]["ACCELERATOR_PEDAL"] / 254.
elif self.CP.flags & HyundaiFlags.HYBRID:
ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
else:
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
ret.gearStep = cp.vl["ELECT_GEAR"]["Elect_Gear_Step"]
if self.CP.carFingerprint == CAR.HYUNDAI_CASPER_EV:
ret.gearStep = 0
elif self.CP.flags & HyundaiFlags.FCEV:
gear = cp.vl["EMS20"]["HYDROGEN_GEAR_SHIFTER"]
elif self.CP.flags & HyundaiFlags.CLUSTER_GEARS:
gear = cp.vl["CLU15"]["CF_Clu_Gear"]
if self.CP.carFingerprint == CAR.KIA_K7:
ret.gearStep = cp.vl["LVR11"]["CF_Lvr_GearInf"]
elif self.CP.flags & HyundaiFlags.TCU_GEARS:
gear = cp.vl["TCU12"]["CUR_GR"]
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
ret.gearStep = cp.vl["LVR11"]["CF_Lvr_GearInf"]
if not self.CP.carFingerprint in (CAR.HYUNDAI_NEXO):
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
else:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
gear_disp = cp.vl["ELECT_GEAR"]
gear_shifter = GearShifter.unknown
if gear == 1546: # Thank you for Neokii # fix PolorBear 22.06.05
gear_shifter = GearShifter.drive
elif gear == 2314:
gear_shifter = GearShifter.neutral
elif gear == 2569:
gear_shifter = GearShifter.park
elif gear == 2566:
gear_shifter = GearShifter.reverse
if gear_shifter != GearShifter.unknown and self.gear_shifter != gear_shifter:
self.gear_shifter = gear_shifter
ret.gearShifter = self.gear_shifter
if not self.CP.flags & HyundaiFlags.CC_ONLY_CAR and (not self.CP.openpilotLongitudinalControl or self.CP.flags & HyundaiFlags.CAMERA_SCC):
aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12"
aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
scc_warning = cp_cruise.vl["SCC12"]["TakeOverReq"] == 1 # sometimes only SCC system shows an FCW
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
if self.CP.carFingerprint == CAR.HYUNDAI_CASPER_EV and aeb_src == "FCA11":
fca_fault = cp_cruise.vl["FCA11"]["FCA_Failinfo"] != 0 or cp_cruise.vl["FCA11"]["FCA_Status"] == 3
if fca_fault:
aeb_warning = False
aeb_braking = False
ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
prev_cruise_buttons = self.cruise_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
#carrot {{
#if self.CRUISE_BUTTON_ALT and cp.vl["CRUISE_BUTTON_ALT"]["SET_ME_1"] == 1:
# self.cruise_buttons_alt = True
cruise_button = [Buttons.NONE]
if self.cruise_buttons_alt:
lfa_button = cp.vl["CRUISE_BUTTON_LFA"]["CruiseSwLfa"]
cruise_button = [Buttons.LFA_BUTTON] if lfa_button > 0 else [cp.vl["CRUISE_BUTTON_ALT"]["CruiseSwState"]]
elif self.HAS_LFA_BUTTON and cp.vl["BCM_PO_11"]["LFA_Pressed"] == 1: # for K5
cruise_button = [Buttons.LFA_BUTTON]
else:
cruise_button = cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]
self.cruise_buttons.extend(cruise_button)
# }} carrot
prev_main_buttons = self.main_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
if self.cruise_buttons_alt:
self.main_buttons.extend(cp.vl_all["CRUISE_BUTTON_ALT"]["CruiseSwMain"])
else:
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
self.mdps12 = copy.copy(cp.vl["MDPS12"])
ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})]
if not self.CP.flags & HyundaiFlags.CC_ONLY_CAR:
tpms_unit = cp.vl["TPMS11"]["UNIT"] * 0.725 if int(cp.vl["TPMS11"]["UNIT"]) > 0 else 1.
ret.tpms.fl = tpms_unit * cp.vl["TPMS11"]["PRESSURE_FL"]
ret.tpms.fr = tpms_unit * cp.vl["TPMS11"]["PRESSURE_FR"]
ret.tpms.rl = tpms_unit * cp.vl["TPMS11"]["PRESSURE_RL"]
ret.tpms.rr = tpms_unit * cp.vl["TPMS11"]["PRESSURE_RR"]
cluSpeed = cp.vl["CLU11"]["CF_Clu_Vanz"]
decimal = cp.vl["CLU11"]["CF_Clu_VanzDecimal"]
if 0. < decimal < 0.5:
cluSpeed += decimal
ret.vEgoCluster = cluSpeed * speed_conv
vEgoClu, aEgoClu = self.update_clu_speed_kf(ret.vEgoCluster)
ret.vCluRatio = (ret.vEgo / vEgoClu) if (vEgoClu > 3. and ret.vEgo > 3.) else 1.0
if self.CP.extFlags & HyundaiExtFlags.NAVI_CLUSTER.value:
speedLimit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
speedLimitCam = cp.vl["Navi_HU"]["SpeedLim_Nav_Cam"]
ret.speedLimit = speedLimit if speedLimit < 255 and speedLimitCam == 1 else 0
speed_limit_cam = speedLimitCam == 1
else:
ret.speedLimit = 0
ret.speedLimitDistance = 0
speed_limit_cam = False
self.update_speed_limit(ret, speed_limit_cam)
if prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
return ret, self.out_iq
def update_speed_limit(self, ret, speed_limit_cam):
self.totalDistance += ret.vEgo * DT_CTRL
if ret.speedLimit > 0 and not ret.gasPressed and speed_limit_cam:
if self.speedLimitDistance <= self.totalDistance:
self.speedLimitDistance = self.totalDistance + ret.speedLimit * 6
self.speedLimitDistance = max(self.totalDistance + 1, self.speedLimitDistance)
else:
self.speedLimitDistance = self.totalDistance
ret.speedLimitDistance = self.speedLimitDistance - self.totalDistance
def update_canfd(self, can_parsers) -> structs.CarState:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
cp_alt = can_parsers[Bus.alt] if Bus.alt in can_parsers else None
ret = structs.CarState()
if self.CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230:
ret.evModeActive, ret.evModeValid = _get_ev_mode_state(cp)
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
if self.CP.flags & (HyundaiFlags.EV | HyundaiFlags.HYBRID):
offset = 255. if self.CP.flags & HyundaiFlags.EV else 1023.
ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset if not self.use_accelerator else 0 if self.accelerator is None else self.accelerator["ACCELERATOR_PEDAL"] / offset
ret.gasPressed = ret.gas > 1e-5
else:
ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"]) if not self.use_accelerator else False if self.accelerator is None else bool(self.accelerator["ACCELERATOR_PEDAL_PRESSED"])
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
#print(cp.vl["TCS"], cp.vl_all["TCS"]["DriverBraking"][-10:])
if self.doors_seatbelts is not None:
ret.doorOpen = self.doors_seatbelts["DRIVER_DOOR"] == 1
ret.seatbeltUnlatched = self.doors_seatbelts["DRIVER_SEATBELT"] == 0
gear = cp.vl[self.gear_msg_canfd]["GEAR"] if not self.use_accelerator else 0 if self.accelerator is None else self.accelerator["GEAR"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
if self.TPMS:
tpms_unit = cp.vl["TPMS"]["UNIT"] * 0.725 if int(cp.vl["TPMS"]["UNIT"]) > 0 else 1.
ret.tpms.fl = tpms_unit * cp.vl["TPMS"]["PRESSURE_FL"]
ret.tpms.fr = tpms_unit * cp.vl["TPMS"]["PRESSURE_FR"]
ret.tpms.rl = tpms_unit * cp.vl["TPMS"]["PRESSURE_RL"]
ret.tpms.rr = tpms_unit * cp.vl["TPMS"]["PRESSURE_RR"]
# TODO: figure out positions
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = all(speed <= STANDSTILL_THRESHOLD for speed in
(ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr))
ret.brakeLights = ret.brakePressed or cp.vl["TCS"]["BrakeLight"] == 1 or ret.aEgo < -0.5
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
# steering angle deg값이 이상함. mdps값이 더 신뢰가 가는듯.. torque steering 차량도 확인해야함.
#ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
#ret.steeringAngleDeg = cp.vl["MDPS"]["STEERING_ANGLE"] * -1
if self.CP.flags & HyundaiFlags.ANGLE_CONTROL:
ret.steeringAngleDeg = cp.vl["MDPS"]["STEERING_ANGLE_2"] * -1
else:
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
trailer_signal = self.trailer_status is not None and self.trailer_status["TRAILER_CONNECTED"] != 0
if trailer_signal:
# Rising edge is immediate so trailer-specific behavior is preserved.
self.trailer_timeout_cnt = 0
self.trailer_connected = True
elif self.trailer_connected:
# During ignition-off, the trailer bit can clear before the cluster shuts down.
# Keep suppression active through that transient, while still detecting a real
# disconnect after 5 seconds at the 100 Hz CarState update rate.
self.trailer_timeout_cnt += 1
if self.trailer_timeout_cnt > TRAILER_DISCONNECT_GRACE_FRAMES:
self.trailer_connected = False
else:
self.trailer_timeout_cnt = 0
ret.trailerConnected = self.trailer_connected
if self.trailer_connected != self.trailer_connected_prev:
print(f"[TRAILER_DEBUG] connected={self.trailer_connected} timeout={self.trailer_timeout_cnt}")
self.trailer_connected_prev = self.trailer_connected
ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 or cp.vl["MDPS"]["LFA2_FAULT"] != 0
#ret.steerFaultTemporary = False
blinkers_info = self.blinkers if self.blinkers is not None else self.blinkers_alt if self.blinkers_alt is not None else None
if blinkers_info is not None:
left_blinker_lamp = blinkers_info["LEFT_LAMP"] or blinkers_info["LEFT_LAMP_ALT"]
right_blinker_lamp = blinkers_info["RIGHT_LAMP"] or blinkers_info["RIGHT_LAMP_ALT"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, left_blinker_lamp, right_blinker_lamp)
if self.CP.enableBsm:
if self.cp_bsm is None:
if 442 in cp.seen_addresses:
self.cp_bsm = cp
print("######## BSM in ECAN")
elif 442 in cp_cam.seen_addresses:
self.cp_bsm = cp_cam
print("######## BSM in CAM")
else:
bsm_info = self.cp_bsm.vl["BLINDSPOTS_REAR_CORNERS"]
ret.leftBlindspot = (bsm_info["FL_INDICATOR"] + bsm_info["INDICATOR_LEFT_TWO"] + bsm_info["INDICATOR_LEFT_FOUR"]) > 0
ret.rightBlindspot = (bsm_info["FR_INDICATOR"] + bsm_info["INDICATOR_RIGHT_TWO"] + bsm_info["INDICATOR_RIGHT_FOUR"]) > 0
# cruise state
if self.cruise_buttons_alt2 is not None:
cruise_button = self.cruise_buttons_alt2["CRUISE_BUTTONS"]
else:
cruise_button = cp.vl[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]
if cruise_button in [Buttons.RES_ACCEL, Buttons.SET_DECEL] and self.CP.openpilotLongitudinalControl:
self.main_enabled = True
# CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
ret.cruiseState.available = self.main_enabled and self.controls_ready_count >= READY_COUNT_OK #cp.vl["TCS"]["ACCEnable"] == 0
if self.CP.flags & HyundaiFlags.CAMERA_SCC.value:
self.MainMode_ACC = cp_cam.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
self.ACCMode = cp_cam.vl["SCC_CONTROL"]["ACCMode"]
self.LFA_ICON = cp_cam.vl["LFAHDA_CLUSTER"]["HDA_LFA_SymSta"]
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
if self.MainMode_ACC or self.main_enabled:
self.main_enabled = True
else:
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
if cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1: # carrot
ret.cruiseState.available = self.main_enabled = True
ret.pcmCruiseGap = int(np.clip(cp_cruise_info.vl["SCC_CONTROL"]["DISTANCE_SETTING"], 1, 4))
ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["InfoDisplay"] >= 4
ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
ret.brakeHoldActive = cp.vl["ESP_STATUS"]["AUTO_HOLD"] == 1 and cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] not in (1, 2)
speed_limit_cam = False
corner = False
corner_infos = [info for info in (self.adrv_0x1ea, self.ccnc_0x162) if info is not None]
if corner_infos:
def corner_max(signal):
return max(info[signal] for info in corner_infos)
ret.leftLongDist = self.lf_distance = corner_max("LF_DETECT_DISTANCE")
ret.rightLongDist = self.rf_distance = corner_max("RF_DETECT_DISTANCE")
self.lr_distance = corner_max("LR_DETECT_DISTANCE")
self.rr_distance = corner_max("RR_DETECT_DISTANCE")
ret.leftLatDist = corner_max("LF_DETECT_LATERAL")
ret.rightLatDist = corner_max("RF_DETECT_LATERAL")
ret.leftRearLongDist = self.lr_distance
ret.rightRearLongDist = self.rr_distance
ret.leftRearLatDist = corner_max("LR_DETECT_LATERAL")
ret.rightRearLatDist = corner_max("RR_DETECT_LATERAL")
corner = True
if corner:
raw_corner_radar_enabled = (
self.op_params.get_int("EnableCornerRadar") > 0 and
bool(self.CP.extFlags & (HyundaiExtFlags.CORNER_RADAR_OBJECTS_235.value |
HyundaiExtFlags.CORNER_RADAR_OBJECTS_180.value))
)
left_front_block = (not raw_corner_radar_enabled) and 0 < ret.leftLongDist < 7.0
right_front_block = (not raw_corner_radar_enabled) and 0 < ret.rightLongDist < 7.0
rear_block_dist = 5.0 if raw_corner_radar_enabled else 7.0
left_rear_block = 0 < self.lr_distance < rear_block_dist
right_rear_block = 0 < self.rr_distance < rear_block_dist
left_block = left_front_block or left_rear_block
right_block = right_front_block or right_rear_block
if left_block:
ret.leftBlindspot = True
if right_block:
ret.rightBlindspot = True
if self.hda_info_4a3 is not None:
speedLimit = self.hda_info_4a3["SPEED_LIMIT"]
if not self.is_metric:
speedLimit *= CV.MPH_TO_KPH
ret.speedLimit = speedLimit if speedLimit < 255 else 0
if int(self.hda_info_4a3["MapSource"]) == 2:
speed_limit_cam = True
if self.time_zone == "UTC":
country_code = int(self.hda_info_4a3["CountryCode"])
self.time_zone = ZoneInfo(NUMERIC_TO_TZ.get(country_code, "UTC"))
ret.gearStep = cp.vl["GEAR"]["GEAR_STEP"] if self.GEAR else 0
if 1 <= ret.gearStep <= 8 and ret.gearShifter == GearShifter.unknown:
ret.gearShifter = GearShifter.drive
ret.gearStep = cp.vl["GEAR_ALT"]["GEAR_STEP"] if self.GEAR_ALT else ret.gearStep
lane_info = self.cam_0x2a4 if self.cam_0x2a4 is not None else self.cam_0x362
if lane_info is not None:
left_lane_prob = lane_info["LEFT_LANE_PROB"]
right_lane_prob = lane_info["RIGHT_LANE_PROB"]
left_lane_type = lane_info["LEFT_LANE_TYPE"] # 0: dashed, 1: solid, 2: undecided, 3: road edge, 4: DLM Inner Solid, 5: DLM InnerDashed, 6:DLM Inner Undecided, 7: Botts Dots, 8: Barrier
right_lane_type = lane_info["RIGHT_LANE_TYPE"]
left_lane_color = lane_info["LEFT_LANE_COLOR"] # 0: none, 1: white, 2: yellow, 3: blue
right_lane_color = lane_info["RIGHT_LANE_COLOR"]
left_lane_info = left_lane_color * 10 + left_lane_type
right_lane_info = right_lane_color * 10 + right_lane_type
ret.leftLaneLine = left_lane_info
ret.rightLaneLine = right_lane_info
# Manual Speed Limit Assist is a feature that replaces non-adaptive cruise control on EV CAN FD platforms.
# It limits the vehicle speed, overridable by pressing the accelerator past a certain point.
# The car will brake, but does not respect positive acceleration commands in this mode
# TODO: find this message on ICE & HYBRID cars + cruise control signals (if exists)
if self.CP.flags & HyundaiFlags.EV:
if self.manual_speed_limit_assist is not None:
#ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
ret.cruiseState.nonAdaptive = self.manual_speed_limit_assist["MSLA_ENABLED"] == 1
if self.LOCAL_TIME and self.time_zone != "UTC":
lt = cp.vl["LOCAL_TIME"]
y, m, d, H, M, S = int(lt["YEAR"]) + 2000, int(lt["MONTH"]), int(lt["DATE"]), int(lt["HOURS"]), int(lt["MINUTES"]), int(lt["SECONDS"])
try:
dt_local = datetime(y, m, d, H, M, S, tzinfo=self.time_zone)
ret.datetime = int(dt_local.timestamp() * 1000)
except:
#print(f"Error parsing local time: {y}-{m}-{d} {H}:{M}:{S} in {self.time_zone}")
pass
prev_cruise_buttons = self.cruise_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
#carrot {{
if self.cruise_buttons_alt2 is not None:
if int(self.cruise_buttons_alt2.get("LFA_BTN", 0)) == 1:
cruise_button = [Buttons.LFA_BUTTON]
else:
v = int(self.cruise_buttons_alt2.get("CRUISE_BUTTONS", 0))
cruise_button = [v if v < 5 else Buttons.NONE]
elif cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]:
cruise_button = [Buttons.LFA_BUTTON]
else:
cruise_button = cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]
self.cruise_buttons.extend(cruise_button)
# }} carrot
#if self.cruise_btns_msg_canfd in cp.vl:
# self.cruise_buttons_msg = copy.copy(cp.vl[self.cruise_btns_msg_canfd])
"""
if self.cruise_btns_msg_canfd in cp.vl: #carrot
if not cp.vl[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]:
pass
#print("empty cruise btns...")
else:
self.cruise_buttons_msg = copy.copy(cp.vl[self.cruise_btns_msg_canfd])
"""
prev_main_buttons = self.main_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
if self.cruise_buttons_alt2 is not None:
self.main_buttons.extend([1 if int(self.cruise_buttons_alt2.get("CRUISE_BUTTONS", 0)) == 8 else 0])
else:
adaptive_main = cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"]
normal_main = cp.vl_all[self.cruise_btns_msg_canfd]["NORMAL_CRUISE_MAIN_BTN"]
self.main_buttons.extend(int(adaptive or normal) for adaptive, normal in zip(adaptive_main, normal_main, strict=True))
if self.main_buttons[-1] != prev_main_buttons and not self.main_buttons[-1]: # and self.CP.openpilotLongitudinalControl: #carrot
self.main_enabled = not self.main_enabled
print("main_enabled = {}".format(self.main_enabled))
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
speed_conv = CV.KPH_TO_MS # if self.is_metric else CV.MPH_TO_MS
cluSpeed = cp.vl["CRUISE_BUTTONS_ALT"]["CLU_SPEED"]
ret.vEgoCluster = cluSpeed * speed_conv # MPH단위에서도 KPH로 나오는듯..
vEgoClu, aEgoClu = self.update_clu_speed_kf(ret.vEgoCluster)
ret.vCluRatio = (ret.vEgo / vEgoClu) if (vEgoClu > 3. and ret.vEgo > 3.) else 1.0
self.update_speed_limit(ret, speed_limit_cam)
paddle_button = self.paddle_button_prev
if self.cruise_btns_msg_canfd == "CRUISE_BUTTONS":
paddle_button = 1 if cp.vl["CRUISE_BUTTONS"]["LEFT_PADDLE"] == 1 else 2 if cp.vl["CRUISE_BUTTONS"]["RIGHT_PADDLE"] == 1 else 0
elif self.gear_msg_canfd == "GEAR":
paddle_button = 1 if cp.vl["GEAR"]["LEFT_PADDLE"] == 1 else 2 if cp.vl["GEAR"]["RIGHT_PADDLE"] == 1 else 0
ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(paddle_button, self.paddle_button_prev, {1: ButtonType.paddleLeft, 2: ButtonType.paddleRight}),
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})]
self.paddle_button_prev = paddle_button
return ret
def get_can_parsers_canfd(self, CP, CP_IQ=None):
msgs = []
if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
# TODO: this can be removed once we add dynamic support to vl_all
msgs += [
("CRUISE_BUTTONS", 50)
]
CAN = CanBus(CP)
pt_parser = CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CAN.ECAN)
if CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230:
# Display-only while the signal is fleet-validated: checksum and freshness
# gate the value without making a counter/alive fault disable controls.
pt_parser._add_message(EV_MODE_STATUS_MSG, math.nan, ignore_counter=True)
return {
Bus.pt: pt_parser,
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CAN.CAM),
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CAN.ACAN),
}
def get_can_parsers(self, CP, CP_IQ=None):
if CP.flags & HyundaiFlags.CANFD:
return self.get_can_parsers_canfd(CP)
return {
# EMS21 carries SCR_UREA_LEVEL on diesel platforms. NaN frequency makes
# it optional, so gasoline/EV platforms do not fail CAN validity.
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [("EMS21", math.nan), ("TPMS11", math.nan)], 0),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
}

1302
artifacts/package_runtime/iqdbc/car/hyundai/fingerprints.py Normal file
View File

File diff suppressed because it is too large Load Diff

403
artifacts/package_runtime/iqdbc/car/hyundai/hyundaican.py Normal file
View File

@@ -0,0 +1,403 @@
import copy
import crcmod
from iqdbc.car.hyundai.values import CAR, HyundaiFlags
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
def suppress_casper_ev_fca11_fault(values):
# CASPER EV can report transient FCA faults during camera-SCC handoff.
# Keep the copied FCA11 frame non-faulting without changing other cars.
fca_fault = values["FCA_Failinfo"] != 0 or values["FCA_Status"] == 3
values["FCA_Failinfo"] = 0
if fca_fault:
values["FCA_Status"] = 2
values["CF_VSM_Prefill"] = 0
values["CF_VSM_HBACmd"] = 0
values["CF_VSM_Warn"] = 0
values["CF_VSM_BeltCmd"] = 0
values["CR_VSM_DecCmd"] = 0
values["FCA_CmdAct"] = 0
values["FCA_StopReq"] = 0
values["CF_VSM_DecCmdAct"] = 0
return values
def create_lkas11(packer, frame, CP, apply_torque, steer_req,
torque_fault, lkas11, sys_warning, sys_state, enabled,
left_lane, right_lane,
left_lane_depart, right_lane_depart, is_ldws_car):
values = {s: lkas11[s] for s in [
"CF_Lkas_LdwsActivemode",
"CF_Lkas_LdwsSysState",
"CF_Lkas_SysWarning",
"CF_Lkas_LdwsLHWarning",
"CF_Lkas_LdwsRHWarning",
"CF_Lkas_HbaLamp",
"CF_Lkas_FcwBasReq",
"CF_Lkas_HbaSysState",
"CF_Lkas_FcwOpt",
"CF_Lkas_HbaOpt",
"CF_Lkas_FcwSysState",
"CF_Lkas_FcwCollisionWarning",
"CF_Lkas_FusionState",
"CF_Lkas_FcwOpt_USM",
"CF_Lkas_LdwsOpt_USM",
]}
values["CF_Lkas_LdwsSysState"] = sys_state
values["CF_Lkas_SysWarning"] = 0 # 3 if sys_warning else 0
values["CF_Lkas_LdwsLHWarning"] = left_lane_depart
values["CF_Lkas_LdwsRHWarning"] = right_lane_depart
values["CR_Lkas_StrToqReq"] = apply_torque
values["CF_Lkas_ActToi"] = steer_req
values["CF_Lkas_ToiFlt"] = torque_fault # seems to allow actuation on CR_Lkas_StrToqReq
values["CF_Lkas_MsgCount"] = frame % 0x10
if CP.flags & HyundaiFlags.SEND_LFA.value or CP.carFingerprint in (CAR.HYUNDAI_SANTA_FE):
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 0 if CP.carFingerprint in (CAR.KIA_RAY_EV) else 2
# FcwOpt_USM 5 = Orange blinking car + lanes
# FcwOpt_USM 4 = Orange car + lanes
# FcwOpt_USM 3 = Green blinking car + lanes
# FcwOpt_USM 2 = Green car + lanes
# FcwOpt_USM 1 = White car + lanes
# FcwOpt_USM 0 = No car + lanes
values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1
# SysWarning 4 = keep hands on wheel
# SysWarning 5 = keep hands on wheel (red)
# SysWarning 6 = keep hands on wheel (red) + beep
# Note: the warning is hidden while the blinkers are on
values["CF_Lkas_SysWarning"] = 0 #4 if sys_warning else 0
# Likely cars lacking the ability to show individual lane lines in the dash
elif CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL):
# SysWarning 4 = keep hands on wheel + beep
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
# SysState 0 = no icons
# SysState 1-2 = white car + lanes
# SysState 3 = green car + lanes, green steering wheel
# SysState 4 = green car + lanes
values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1
values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition
# these have no effect
values["CF_Lkas_LdwsActivemode"] = 0
values["CF_Lkas_FcwOpt_USM"] = 0
elif CP.carFingerprint == CAR.HYUNDAI_GENESIS:
# This field is actually LdwsActivemode
# Genesis and Optima fault when forwarding while engaged
values["CF_Lkas_LdwsActivemode"] = 2
if is_ldws_car:
values["CF_Lkas_LdwsOpt_USM"] = 3
values["CF_Lkas_Chksum"] = 0
dat = packer.make_can_msg("LKAS11", 0, values)[1]
if CP.flags & HyundaiFlags.CHECKSUM_CRC8:
# CRC Checksum as seen on 2019 Hyundai Santa Fe
dat = dat[:6] + dat[7:8]
checksum = hyundai_checksum(dat)
elif CP.flags & HyundaiFlags.CHECKSUM_6B:
# Checksum of first 6 Bytes, as seen on 2018 Kia Sorento
checksum = sum(dat[:6]) % 256
else:
# Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger
checksum = (sum(dat[:6]) + dat[7]) % 256
values["CF_Lkas_Chksum"] = checksum
return packer.make_can_msg("LKAS11", 0, values)
def create_clu11(packer, frame, clu11, button, CP):
values = {s: clu11[s] for s in [
"CF_Clu_CruiseSwState",
"CF_Clu_CruiseSwMain",
"CF_Clu_SldMainSW",
"CF_Clu_ParityBit1",
"CF_Clu_VanzDecimal",
"CF_Clu_Vanz",
"CF_Clu_SPEED_UNIT",
"CF_Clu_DetentOut",
"CF_Clu_RheostatLevel",
"CF_Clu_CluInfo",
"CF_Clu_AmpInfo",
"CF_Clu_AliveCnt1",
]}
values["CF_Clu_CruiseSwState"] = button
values["CF_Clu_AliveCnt1"] = frame % 0x10
# send buttons to camera on camera-scc based cars
bus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC else 0
return packer.make_can_msg("CLU11", bus, values)
def create_lfahda_mfc(packer, CC, blinking_signal):
activeCarrot = CC.hudControl.activeCarrot
values = {
"LFA_Icon_State": 2 if CC.latActive else 1 if CC.enabled else 0,
#"HDA_Active": 1 if activeCarrot >= 2 else 0,
#"HDA_Icon_State": 2 if activeCarrot == 3 and blinking_signal else 2 if activeCarrot >= 2 else 0,
"HDA_Icon_State": 0 if activeCarrot == 3 and blinking_signal else 2 if activeCarrot >= 1 else 0,
"HDA_VSetReq": 0, #set_speed_in_units if activeCarrot >= 2 else 0,
"HDA_USM" : 2,
"HDA_Icon_Wheel" : 1 if CC.latActive else 0,
#"HDA_Chime" : 1 if CC.latActive else 0, # comment for K9 chime,
}
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands_scc(packer, enabled, accel, jerk, idx, hud_control, set_speed, stopping, long_override, suppress_casper_ev_fca, CS, soft_hold_mode):
from iqdbc.car.hyundai.carcontroller import HyundaiJerk
cruise_available = CS.out.cruiseState.available
if CS.paddle_button_prev > 0:
cruise_available = False
soft_hold_active = CS.softHoldActive
soft_hold_info = soft_hold_active > 1 and enabled
#soft_hold_mode = 2 ## some cars can't enable while braking
long_enabled = enabled or (soft_hold_active > 0 and soft_hold_mode == 2)
stop_req = 1 if stopping or (soft_hold_active > 0 and soft_hold_mode == 2) else 0
d = hud_control.leadDistance
objGap = 0 if d == 0 else 2 if d < 25 else 3 if d < 40 else 4 if d < 70 else 5
objGap2 = 0 if objGap == 0 else 2 if hud_control.leadRelSpeed < -0.2 else 1
if long_enabled:
if jerk.carrot_cruise == 1:
long_enabled = False
accel = -0.5
elif jerk.carrot_cruise == 2:
accel = jerk.carrot_cruise_accel
if long_enabled:
scc12_acc_mode = 2 if long_override else 1
scc14_acc_mode = 2 if long_override else 1
if CS.out.brakeHoldActive:
scc12_acc_mode = 0
scc14_acc_mode = 4
elif CS.out.brakePressed:
scc12_acc_mode = 1
scc14_acc_mode = 1
else:
scc12_acc_mode = 0
scc14_acc_mode = 4
warning_front = False
commands = []
if CS.scc11 is not None:
values = copy.copy(CS.scc11)
values["MainMode_ACC"] = 1 if cruise_available else 0
values["TauGapSet"] = hud_control.leadDistanceBars
values["VSetDis"] = set_speed if enabled else 0
values["AliveCounterACC"] = idx % 0x10
values["SCCInfoDisplay"] = 3 if warning_front else 4 if soft_hold_info else 0 if enabled else 0 #2: 크루즈 선택, 3: 전방상황주의, 4: 출발준비
values["ObjValid"] = 1 if hud_control.leadVisible else 0
values["ACC_ObjStatus"] = 1 if hud_control.leadVisible else 0
values["ACC_ObjLatPos"] = 0
values["ACC_ObjRelSpd"] = hud_control.leadRelSpeed
values["ACC_ObjDist"] = int(hud_control.leadDistance)
values["DriverAlertDisplay"] = 0
commands.append(packer.make_can_msg("SCC11", 0, values))
if CS.scc12 is not None:
values = copy.copy(CS.scc12)
values["ACCMode"] = scc12_acc_mode #2 if enabled and long_override else 1 if long_enabled else 0
values["StopReq"] = stop_req
values["aReqRaw"] = accel
values["aReqValue"] = accel
values["ACCFailInfo"] = 0
#values["DESIRED_DIST"] = CS.out.vEgo * 1.0 + 4.0 # TF: 1.0 + STOPDISTANCE 4.0 m로 가정함.
values["CR_VSM_ChkSum"] = 0
values["CR_VSM_Alive"] = idx % 0xF
scc12_dat = packer.make_can_msg("SCC12", 0, values)[1]
values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
commands.append(packer.make_can_msg("SCC12", 0, values))
if CS.scc14 is not None:
values = copy.copy(CS.scc14)
values["ComfortBandUpper"] = jerk.cb_upper
values["ComfortBandLower"] = jerk.cb_lower
values["JerkUpperLimit"] = jerk.jerk_u
values["JerkLowerLimit"] = jerk.jerk_l if long_enabled else 0 # for KONA test
values["ACCMode"] = scc14_acc_mode #2 if enabled and long_override else 1 if long_enabled else 4 # stock will always be 4 instead of 0 after first disengage
values["ObjGap"] = objGap #2 if hud_control.leadVisible else 0 # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
values["ObjDistStat"] = objGap2
commands.append(packer.make_can_msg("SCC14", 0, values))
if CS.fca11 is not None and suppress_casper_ev_fca: # CASPER_EV의 경우 FCA11에서 fail이 간헐적 발생함.. 그냥막자.. 원인불명..
values = suppress_casper_ev_fca11_fault(copy.copy(CS.fca11))
fca11_dat = packer.make_can_msg("FCA11", 0, values)[1]
values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, values))
# Only send FCA11 on cars where it exists on the bus
if False: #use_fca:
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/iqdbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
"CR_FCA_Alive": idx % 0xF,
"PAINT1_Status": 1,
"FCA_DrvSetStatus": 1,
"FCA_Status": 1, # AEB disabled
}
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[1]
fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
return commands
def create_acc_opt_copy(CS, packer):
values = copy.copy(CS.scc13)
if values["NEW_SIGNAL_1"] == 255:
values["NEW_SIGNAL_1"] = 218
values["NEW_SIGNAL_2"] = 0
return packer.make_can_msg("SCC13", 0, CS.scc13)
def create_acc_commands(packer, enabled, accel, jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP, CS, soft_hold_mode):
from iqdbc.car.hyundai.carcontroller import HyundaiJerk
cruise_available = CS.out.cruiseState.available
soft_hold_active = CS.softHoldActive
soft_hold_info = soft_hold_active > 1 and enabled
#soft_hold_mode = 2 ## some cars can't enable while braking
long_enabled = enabled or (soft_hold_active > 0 and soft_hold_mode == 2)
stop_req = 1 if stopping or (soft_hold_active > 0 and soft_hold_mode == 2) else 0
d = hud_control.leadDistance
objGap = 0 if d == 0 else 2 if d < 25 else 3 if d < 40 else 4 if d < 70 else 5
objGap2 = 0 if objGap == 0 else 2 if hud_control.leadRelSpeed < -0.2 else 1
if long_enabled:
scc12_acc_mode = 2 if long_override else 1
scc14_acc_mode = 2 if long_override else 1
if CS.out.brakeHoldActive:
scc12_acc_mode = 0
scc14_acc_mode = 4
elif CS.out.brakePressed:
scc12_acc_mode = 1
scc14_acc_mode = 1
else:
scc12_acc_mode = 0
scc14_acc_mode = 4
warning_front = False
commands = []
scc11_values = {
"MainMode_ACC": 1 if cruise_available else 0,
"TauGapSet": hud_control.leadDistanceBars,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,
"SCCInfoDisplay": 3 if warning_front else 4 if soft_hold_info else 0 if enabled else 0,
"ObjValid": 1 if hud_control.leadVisible else 0, # close lead makes controls tighter
"ACC_ObjStatus": 1 if hud_control.leadVisible else 0, # close lead makes controls tighter
"ACC_ObjLatPos": 0,
"ACC_ObjRelSpd": hud_control.leadRelSpeed,
"ACC_ObjDist": int(hud_control.leadDistance), # close lead makes controls tighter
"DriverAlertDisplay": 0,
}
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
scc12_values = {
"ACCMode": scc12_acc_mode,
"StopReq": stop_req,
"aReqRaw": 0 if stop_req > 0 else accel,
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
#"DESIRED_DIST": CS.out.vEgo * 1.0 + 4.0,
"CR_VSM_Alive": idx % 0xF,
}
# show AEB disabled indicator on dash with SCC12 if not sending FCA messages.
# these signals also prevent a TCS fault on non-FCA cars with alpha longitudinal
if not use_fca:
scc12_values["CF_VSM_ConfMode"] = 1
scc12_values["AEB_Status"] = 1 # AEB disabled
scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[1]
scc12_values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
scc14_values = {
"ComfortBandUpper": jerk.cb_upper, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": jerk.cb_lower, # stock usually is 0 but sometimes uses higher values
"JerkUpperLimit": jerk.jerk_u, # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": jerk.jerk_l, # stock usually is 0.5 but sometimes uses higher values
"ACCMode": scc14_acc_mode, # if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
"ObjGap": objGap, #2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
"ObjDistStat": objGap2,
}
commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
# Only send FCA11 on cars where it exists on the bus
# On Camera SCC cars, FCA11 is not disabled, so we forward stock FCA11 back to the car forward hooks
if use_fca and not (CP.flags & HyundaiFlags.CAMERA_SCC):
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/iqdbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
"CR_FCA_Alive": idx % 0xF,
"PAINT1_Status": 1,
"FCA_DrvSetStatus": 1,
"FCA_Status": 1, # AEB disabled
}
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[1]
fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
return commands
def create_acc_opt(packer, CP):
commands = []
scc13_values = {
"SCCDrvModeRValue": 2,
"SCC_Equip": 1,
"Lead_Veh_Dep_Alert_USM": 2,
}
commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
# TODO: this needs to be detected and conditionally sent on unsupported long cars
# On Camera SCC cars, FCA12 is not disabled, so we forward stock FCA12 back to the car forward hooks
if not (CP.flags & HyundaiFlags.CAMERA_SCC):
fca12_values = {
"FCA_DrvSetState": 2,
"FCA_USM": 1, # AEB disabled
}
commands.append(packer.make_can_msg("FCA12", 0, fca12_values))
return commands
def create_frt_radar_opt(packer):
frt_radar11_values = {
"CF_FCA_Equip_Front_Radar": 1,
}
return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values)
def create_clu11_button(packer, frame, clu11, button, CP):
values = clu11.copy()
values["CF_Clu_CruiseSwState"] = button
#values["CF_Clu_AliveCnt1"] = frame % 0x10
values["CF_Clu_AliveCnt1"] = (values["CF_Clu_AliveCnt1"] + 1) % 0x10
# send buttons to camera on camera-scc based cars
bus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC else 0
return packer.make_can_msg("CLU11", bus, values)
def create_mdps12(packer, frame, mdps12):
values = mdps12
values["CF_Mdps_ToiActive"] = 0
values["CF_Mdps_ToiUnavail"] = 1
values["CF_Mdps_MsgCount2"] = frame % 0x100
values["CF_Mdps_Chksum2"] = 0
dat = packer.make_can_msg("MDPS12", 2, values)[1]
checksum = sum(dat) % 256
values["CF_Mdps_Chksum2"] = checksum
return packer.make_can_msg("MDPS12", 2, values)

830
artifacts/package_runtime/iqdbc/car/hyundai/hyundaicanfd.py Normal file
View File

@@ -0,0 +1,830 @@
import copy
import numpy as np
from iqdbc.car import CanBusBase
from iqdbc.car.crc import CRC16_XMODEM
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiExtFlags
from iqpilot.common.params import Params
from iqdbc.car.common.conversions import Conversions as CV
from iqpilot.cereal import log
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
TurnDirection = log.Desire
class CanBus(CanBusBase):
def __init__(self, CP, fingerprint=None, lka_steering=None) -> None:
super().__init__(CP, fingerprint)
if lka_steering is None:
lka_steering = CP.flags & HyundaiFlags.CANFD_HDA2.value if CP is not None else False
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. LKA steering cars
# have a different harness than the LFA steering variants in order to split
# a different bus, since the steering is done by different ECUs.
self._a, self._e = 1, 0
if lka_steering and Params().get_int("HyundaiCameraSCC") == 0: #배선개조는 무조건 Bus0가 ECAN임.
self._a, self._e = 0, 1
self._a += self.offset
self._e += self.offset
self._cam = 2 + self.offset
@property
def ECAN(self):
return self._e
@property
def ACAN(self):
return self._a
@property
def CAM(self):
return self._cam
# CAN LIST (CAM) - 롱컨개조시... ADAS + CAM
# 160: ADRV_0x160
# 1da: ADRV_0x1da
# 1ea: ADRV_0x1ea
# 200: ADRV_0x200
# 345: ADRV_0x345
# 1fa: CLUSTER_SPEED_LIMIT
# 12a: LFA
# 1e0: LFAHDA_CLUSTER
# 11a:
# 1b5:
# 1a0: SCC_CONTROL
# CAN LIST (ACAN)
# 160: ADRV_0x160
# 51: ADRV_0x51
# 180: CAM_0x180
# ...
# 185: CAM_0x185
# 1b6: CAM_0x1b6
# ...
# 1b9: CAM_0x1b9
# 1fb: CAM_0x1fb
# 2a2 - 2a4
# 2bb - 2be
# LKAS
# 201 - 2a0
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer, apply_angle, max_torque, angle_control):
ret = []
if angle_control:
values = {
"LKA_MODE": 0,
"LKA_ICON": 2 if enabled else 1,
"TORQUE_REQUEST": 0, # apply_steer,
"VALUE63": 0, # LKA_ASSIST
"STEER_REQ": 0, # 1 if lat_active else 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
"LKA_ACTIVE": 3 if lat_active else 0, # this changes sometimes, 3 seems to indicate engaged
"VALUE64": 0, #STEER_MODE, NEW_SIGNAL_2
"LKAS_ANGLE_CMD": -apply_angle,
"LKAS_ANGLE_ACTIVE": 2 if lat_active else 1,
"LKAS_ANGLE_MAX_TORQUE": max_torque if lat_active else 0,
# test for EV6PE
"NEW_SIGNAL_1": 10, #2,
"DampingGain": 9,
"VALUE231": 146,
"VALUE239": 1,
"VALUE247": 255,
"VALUE255": 255,
}
else:
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"TORQUE_REQUEST": apply_steer,
"DampingGain": 100, #3 if enabled else 100,
"STEER_REQ": 1 if lat_active else 0,
#"STEER_MODE": 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
"VALUE63": 0,
"VALUE64": 100,
}
if CP.flags & HyundaiFlags.CANFD_HDA2:
lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "LKAS"
if CP.openpilotLongitudinalControl:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
if not (CP.flags & HyundaiFlags.CAMERA_SCC.value):
ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, values))
else:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
return ret
def create_suppress_lfa(packer, CAN, CS):
if CS.cam_0x362 is not None:
suppress_msg = "CAM_0x362"
lfa_block_msg = CS.cam_0x362
elif CS.cam_0x2a4 is not None:
suppress_msg = "CAM_0x2a4"
lfa_block_msg = CS.cam_0x2a4
else:
return []
#values = {f"BYTE{i}": lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7}
values = copy.copy(lfa_block_msg)
values["COUNTER"] = lfa_block_msg["COUNTER"]
values["SET_ME_0"] = 0
values["SET_ME_0_2"] = 0
values["LEFT_LANE_LINE"] = 0
values["RIGHT_LANE_LINE"] = 0
return [packer.make_can_msg(suppress_msg, CAN.ACAN, values)]
def create_buttons(packer, CP, CAN, cnt, btn):
values = {
"COUNTER": cnt,
"SET_ME_1": 1,
"CRUISE_BUTTONS": btn,
}
#bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
bus = CAN.ECAN
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
# TODO: why do we copy different values here?
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"NEW_SIGNAL_1",
"MainMode_ACC",
"ACCMode",
"ZEROS_9",
"CRUISE_STANDSTILL",
"ZEROS_5",
"DISTANCE_SETTING",
"VSetDis",
]}
else:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"ACCMode",
"VSetDis",
"CRUISE_STANDSTILL",
]}
values.update({
"ACCMode": 4,
"aReqRaw": 0.0,
"aReqValue": 0.0,
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CS, CAN, long_active, lat_active):
if CS.lfahda_cluster is not None:
values = copy.copy(CS.lfahda_cluster)
rx_counter = values.pop("COUNTER", None)
else:
return []
values = {}
rx_counter = None
values["LFA_OptUsmSta"] = 2
values["HDA_OptUsmSta"] = 2
values["HDA_CntrlModSta"] = 2 if long_active else 0
values["HDA_LFA_SymSta"] = 2 if lat_active else 0
return [packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values, rx_counter=rx_counter)]
def create_lfa_icon_non_camera_scc(packer, CS, CAN, CC):
ret = []
if CS.adrv_0x161 is not None:
values = copy.copy(CS.adrv_0x161)
rx_counter = values.pop("COUNTER", None)
lat_active = CC.latActive
lat_enabled = CS.out.latEnabled
values["LFA_ICON"] = 2 if lat_active else 1 if lat_enabled else 0
values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 0
if values["ALERTS_2"] in [1, 2, 5, 6, 10, 21, 22]:
values["ALERTS_2"] = 0
values["DAW_ICON"] = 0
if values["ALERTS_1"] == 0:
values["SOUNDS_1"] = 0
values["SOUNDS_2"] = 0
values["SOUNDS_4"] = 0
if values["ALERTS_3"] in [3, 4, 11, 12, 13, 14, 17, 19, 26, 7, 8, 9, 10]:
values["ALERTS_3"] = 0
values["SOUNDS_3"] = 0
if values["ALERTS_5"] in [1, 2, 3, 4, 5]:
values["ALERTS_5"] = 0
ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values, rx_counter=rx_counter))
return ret
def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, hyundai_jerk, CS):
if CS.scc_control is None:
return None
enabled = (enabled or CS.softHoldActive > 0) and CS.paddle_button_prev == 0
acc_mode = 0 if not enabled else (2 if gas_override else 1)
if hyundai_jerk.carrot_cruise == 1:
acc_mode = 4 if enabled else 0
enabled = False
accel = accel_last = 0.5
elif hyundai_jerk.carrot_cruise == 2:
accel = accel_last = hyundai_jerk.carrot_cruise_accel
jerk_u = hyundai_jerk.jerk_u
jerk_l = hyundai_jerk.jerk_l
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
a_val, a_raw = 0, 0
else:
a_raw = accel
a_val = accel #np.clip(accel, accel_last - jn, accel_last + jn)
values = copy.copy(CS.scc_control)
rx_counter = values.pop("COUNTER", None)
values["ACCMode"] = acc_mode
values["MainMode_ACC"] = 1
values["StopReq"] = 1 if stopping or CS.softHoldActive > 0 else 0 # 1: Stop control is required, 2: Not used, 3: Error Indicator
values["aReqValue"] = a_val
values["aReqRaw"] = a_raw
values["VSetDis"] = set_speed
#values["JerkLowerLimit"] = jerk if enabled else 1
#values["JerkUpperLimit"] = 3.0
values["JerkLowerLimit"] = jerk_l if enabled else 1
values["JerkUpperLimit"] = 2.0 if stopping or CS.softHoldActive else jerk_u
values["DISTANCE_SETTING"] = hud_control.leadDistanceBars # + 5
#values["DISTANCE_SETTING"] = hud_control.leadDistanceBars + 5
#values["ACC_ObjDist"] = 1
#values["ObjValid"] = 0
#values["OBJ_STATUS"] = 2
#values["NSCCOper"] = 1 if enabled else 0 # 0: off, 1: Ready, 2: Act, 3: Error Indicator
#values["NSCCOnOff"] = 2 # 0: Default, 1: Off, 2: On, 3: Invalid
#values["SET_ME_3"] = 0x3 # objRelsped와 충돌
#values["ACC_ObjLatPos"] = - hud_control.leadDPath
values["DriveMode"] = 0 # 0: Default, 1: Comfort Mode, 2:Normal mode, 3:Dynamic mode, reserved
hud_lead_info = 0
if hud_control.leadVisible:
hud_lead_info = 1 if values["ACC_ObjRelSpd"] > 0 else 2
values["HUD_LEAD_INFO"] = hud_lead_info #1: in-path object detected(uncontrollable), 2: controllable long, 3: controllable long & lat, ... reserved
values["DriverAlert"] = 0 # 1: SCC Disengaged, 2: No SCC Engage condition, 3: SCC Disenganed when the vehicle stops
values["TARGET_DISTANCE"] = CS.out.vEgo * 1.0 + 4.0
soft_hold_info = 1 if CS.softHoldActive > 1 and enabled else 0
# 이거안하면 정지중 뒤로 밀리는 현상 발생하는듯.. (신호정지중에 뒤로 밀리는 경험함.. 시험해봐야)
if values["InfoDisplay"] != 5: #5: Front Car Departure Notice
values["InfoDisplay"] = 4 if stopping and CS.out.aEgo > -0.3 else 0 # 1: SCC Mode, 2: Convention Cruise Mode, 3: Object disappered at low speed, 4: Available to resume acceleration control, 5: Front vehicle departure notice, 6: Reserved, 7: Invalid
values["TakeOverReq"] = 0 # 1: Takeover request, 2: Not used, 3: Error indicator , 이것이 켜지면 가속을 안하는듯함.
#values["NEW_SIGNAL_4"] = 9 if hud_control.leadVisible else 0
# AccelLimitBandUpper, Lower
values["SysFailState"] = 0 # 1: Performance degredation, 2: system temporairy unavailble, 3: SCC Service required , 눈이 묻어 레이더오류시... 2가 됨. 이때 가속을 안함...
values["AccelLimitBandUpper"] = 0.0 # 이값이 1.26일때 가속을 안하는 증상이 보임..
values["AccelLimitBandLower"] = 0.0
values["ZEROS_7"] = 1
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, jerk_u, jerk_l, CS):
enabled = enabled or CS.softHoldActive > 0
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
a_val, a_raw = 0, 0
else:
a_raw = accel
a_val = np.clip(accel, accel_last - jn, accel_last + jn)
values = {
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
"MainMode_ACC": 1,
"StopReq": 1 if stopping or CS.softHoldActive > 0 else 0,
"aReqValue": a_val,
"aReqRaw": a_raw,
"VSetDis": set_speed,
#"JerkLowerLimit": jerk if enabled else 1,
#"JerkUpperLimit": 3.0,
"JerkLowerLimit": jerk_l if enabled else 1,
"JerkUpperLimit": jerk_u,
"ACC_ObjDist": 1,
#"ObjValid": 0,
#"OBJ_STATUS": 2,
"NSCCOper": 0,
"NSCCOnOff": 2,
"DriveMode": 0,
#"SET_ME_3": 0x3,
"ACC_ObjLatPos": 0x64,
"DISTANCE_SETTING": hud_control.leadDistanceBars, # + 5,
"InfoDisplay": 4 if stopping and CS.out.cruiseState.standstill else 0,
}
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
ret = []
values = {
}
ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values))
blink = 0
if left_blink:
blink = 3
elif right_blink:
blink = 4
values = {
"BLINKER_CONTROL": blink,
}
ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values))
return ret
def create_fca_warning_light(CP, packer, CAN, frame):
ret = []
if CP.flags & HyundaiFlags.CAMERA_SCC.value:
return ret
if frame % 2 == 0:
values = {
'AEB_SETTING': 0x1, # show AEB disabled icon
'SET_ME_2': 0x2,
'SET_ME_FF': 0xff,
'SET_ME_FC': 0xfc,
'SET_ME_9': 0x9,
#'DATA102': 1,
}
ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
return ret
def create_tcs_messages(packer, CAN, CS):
ret = []
if CS.tcs is not None:
values = copy.copy(CS.tcs)
#rx_counter = values.pop("COUNTER", None)
values["DriverBraking"] = 0
values["NEW_SIGNAL_20"] = 0
values["NEW_SIGNAL_11"] = 0
values["DriverBrakingLowSens"] = 0
#values["NEW_SIGNAL_1"] = 0 # accel과 관련.. 옆두부 꺼지는것과 관련? 확인필요
#values["ACC_REQ"] = 1 # 옆두부 꺼지는것과 관련? 확인필요.. 항상 켜지게함..
values["NEW_SIGNAL_1"] = 0 if values["ACC_REQ"] == 1 else 1 # 옆두부..
#ret.append(packer.make_can_msg("TCS", CAN.CAM, values, rx_counter = rx_counter))
ret.append(packer.make_can_msg("TCS", CAN.CAM, values))
return ret
def forward_button_message(packer, CAN, frame, CS, cruise_button, MainMode_ACC_trigger, LFA_trigger):
ret = []
if frame % 2 == 0:
if CS.cruise_buttons_msg is not None:
values = copy.copy(CS.cruise_buttons_msg)
# A held MAIN is reported on this bit and switches some clusters to LIMIT mode.
values["NORMAL_CRUISE_MAIN_BTN"] = 0
#rx_counter = values.pop("COUNTER", None)
cruise_button_driver = values["CRUISE_BUTTONS"]
if cruise_button_driver == 0:
values["CRUISE_BUTTONS"] = cruise_button
if MainMode_ACC_trigger > 0:
#values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
pass
elif LFA_trigger > 0:
values["LFA_BTN"] = 1
#ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values, rx_counter = rx_counter))
ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
return ret
def create_adrv_messages(CP, packer, CAN, frame):
# messages needed to car happy after disabling
# the ADAS Driving ECU to do longitudinal control
ret = []
if not CP.flags & HyundaiFlags.CAMERA_SCC.value:
values = {}
ret.extend(create_fca_warning_light(CP, packer, CAN, frame))
if frame % 5 == 0:
values = {
#'HDA_MODE1': 0x8,
'HDA_MODE2': 0x1,
#'SET_ME_1C': 0x1c,
'SET_ME_FF': 0xff,
#'SET_ME_TMP_F': 0xf,
#'SET_ME_TMP_F_2': 0xf,
#'DATA26': 1, #1
#'DATA32': 5, #5
}
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))
values = {
'SET_ME_E1': 0xe1,
#'SET_ME_3A': 0x3a,
'TauGapSet' : 1,
'NEW_SIGNAL_2': 3,
}
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values))
if frame % 20 == 0:
values = {
'SET_ME_15': 0x15,
}
ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values))
if frame % 100 == 0:
values = {
'SET_ME_22': 0x22,
'SET_ME_41': 0x41,
}
ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values))
return ret
## carrot
def alt_cruise_buttons(packer, CP, CAN, buttons, cruise_btns_msg, cnt):
cruise_btns_msg["CRUISE_BUTTONS"] = buttons
cruise_btns_msg["COUNTER"] = (cruise_btns_msg["COUNTER"] + 1 + cnt) % 256
bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
return packer.make_can_msg("CRUISE_BUTTONS_ALT", bus, cruise_btns_msg)
def hkg_can_fd_checksum(address: int, sig, d: bytearray) -> int:
crc = 0
for i in range(2, len(d)):
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ d[i]]) & 0xFFFF
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ ((address >> 0) & 0xFF)]) & 0xFFFF
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ ((address >> 8) & 0xFF)]) & 0xFFFF
if len(d) == 8:
crc ^= 0x5F29
elif len(d) == 16:
crc ^= 0x041D
elif len(d) == 24:
crc ^= 0x819D
elif len(d) == 32:
crc ^= 0x9F5B
return crc
def _clip_int(x, lo, hi):
return lo if x < lo else hi if x > hi else int(x)
def _get_desire_and_lane_changing(md):
desire = 0
lane_changing = 0
if md is not None:
desire = md.meta.desire.raw
ds = md.meta.desireState
if len(ds) > 4:
if ds[1] > 0.9: lane_changing = 1
if ds[2] > 0.9: lane_changing = 2
if ds[3] > 0.9: lane_changing = 3
if ds[4] > 0.9: lane_changing = 4
return desire, lane_changing
def _apply_lane_desire(values, desire):
#values['LANE_CHANGING'] = 0
if desire == 1: # 좌회전
values['LANE_CHANGING'] = 1
values["LANELINE_CURVATURE"] = 15
values["LANELINE_CURVATURE_DIRECTION"] = 0
elif desire == 2: # 우회전
values['LANE_CHANGING'] = 2
values["LANELINE_CURVATURE"] = 15
values["LANELINE_CURVATURE_DIRECTION"] = 1
elif desire == 3: # 좌차선변경
values['LANE_CHANGING'] = 3
elif desire == 4: # 우차선변경
values['LANE_CHANGING'] = 4
def _apply_radar_blink(values, radar_pairs, frame, *,
disp_dist=30.0, min_dist=14.0,
max_interval=100, t=1.0):
"""
거리 > min_dist 일 때만 깜빡임.
거리 멀수록 interval 커짐(느리게).
"""
for det_key, dist_key in radar_pairs:
dist = values[dist_key]
if dist <= min_dist:
continue
d = min(dist, disp_dist)
interval = int((1 + (max_interval - 1) * (d / disp_dist)) * t)
interval = _clip_int(interval, 1, max_interval)
blink = (frame // interval) & 1
values[det_key] = 2 - blink
values[dist_key] = min_dist
def _suppress_trailer_mode_warning(values, CS):
# Logs from IONIQ 9 show ALERTS_5=6 is the periodic
# "driver assistance limited in trailer mode" popup.
if CS.trailer_connected and values.get("ALERTS_5") == 6:
values["ALERTS_5"] = 0
def _make_ccnc_values(values, CS, lat_active, frame, hud_control,
lane_line=True, corner_radar=True,
desire=0,
blink_pairs=None,
blink_t=1.0):
if lane_line:
curvature = round(CS.out.steeringAngleDeg / 3)
mag = min(abs(curvature), 15)
curv = mag + (-1 if curvature < 0 else 0)
direction = 1 if curvature < 0 else 0
values["LANELINE_CURVATURE"] = curv if lat_active else 0
values["LANELINE_CURVATURE_DIRECTION"] = direction if lat_active else 0
if desire:
_apply_lane_desire(values, desire)
if corner_radar:
radar_all = [
('LF_DETECT', 'LF_DETECT_DISTANCE'),
('RF_DETECT', 'RF_DETECT_DISTANCE'),
('LR_DETECT', 'LR_DETECT_DISTANCE'),
('RR_DETECT', 'RR_DETECT_DISTANCE'),
]
for det_key, dist_key in radar_all:
if values[det_key] >= 4 and values[dist_key] != 0:
values[det_key] = 1
if blink_pairs:
_apply_radar_blink(values, blink_pairs, frame, t=blink_t)
def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control,
disp_angle, left_lane_warning, right_lane_warning,
enable_corner_radar, stopping, canfd_debug):
ret = []
md = CS.modelV2
if not hasattr(create_ccnc_messages, '_lane_line_check') or frame % 100 == 0:
create_ccnc_messages._lane_line_check = Params().get_int("LaneLineCheck")
lane_line_check = create_ccnc_messages._lane_line_check
desire, lane_changing = _get_desire_and_lane_changing(md)
if CP.flags & HyundaiFlags.CAMERA_SCC.value:
HDA_CntrlModSta = 0
HDA_LFA_SymSta = 0
if CS.lfahda_cluster is not None:
HDA_CntrlModSta = CS.lfahda_cluster["HDA_CntrlModSta"]
HDA_LFA_SymSta = CS.lfahda_cluster["HDA_LFA_SymSta"]
if frame % 2 == 0:
#if CS.adrv_0x160 is not None:
# values = copy.copy(CS.adrv_0x160)
# ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
if CS.cruise_buttons_msg is not None:
values = copy.copy(CS.cruise_buttons_msg)
# Keep the physical long press on ECAN for CarState, but don't forward it to CAM.
values["NORMAL_CRUISE_MAIN_BTN"] = 0
if HDA_LFA_SymSta == 0 and 0 < frame % 200 < 12:
values["LFA_BTN"] = 1
if CC.enabled:
if not CS.MainMode_ACC:
if 10 < frame % 200 <= 16 and CS.out.vEgo > 3.:
values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
elif CS.ACCMode in [0, 4]:
if 10 < frame % 200 <= 16 and CS.out.vEgo > 3.:
values["CRUISE_BUTTONS"] = 2
elif CS.scc_control is not None and CS.scc_control["InfoDisplay"] == 4:
if 10 < frame % 30 <= 16 and not stopping:
values["CRUISE_BUTTONS"] = 2
else:
if CS.adrv_0x1ea is not None and CS.adrv_0x1ea["HDA_MODE2"] == 0: # if corner radar is disabled, send main btn
if 10 < frame % 1000 <= 16 and CS.out.vEgo > 3:
values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
# --- 0x161/0x200/0x1ea/0x162 (frame%5) ---
if frame % 5 == 0:
lat_active = CC.latActive
if CS.adrv_0x161 is not None:
main_enabled = CS.out.cruiseState.available
cruise_enabled = CC.enabled
lat_enabled = CS.out.latEnabled
nav_active = hud_control.activeCarrot > 1
# hdpuse carrot
hdp_use = int(Params().get("HDPuse"))
hdp_active = False
if hdp_use == 1:
hdp_active = cruise_enabled and nav_active
elif hdp_use == 2:
hdp_active = cruise_enabled
# hdpuse carrot
values = copy.copy(CS.adrv_0x161)
rx_counter = values.pop("COUNTER", None)
values["SETSPEED"] = (6 if hdp_active else 3 if cruise_enabled else 1) if main_enabled else 0
values["SETSPEED_HUD"] = (5 if hdp_active else 3 if cruise_enabled else 1) if main_enabled else 0
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
values["vSetDis"] = int(set_speed_in_units + 0.5)
values["DISTANCE"] = 4 if hdp_active else hud_control.leadDistanceBars
values["DISTANCE_LEAD"] = 2 if cruise_enabled and hud_control.leadVisible else 1 if main_enabled and hud_control.leadVisible else 0
values["DISTANCE_CAR"] = 3 if hdp_active else 2 if cruise_enabled else 1 if main_enabled else 0
values["DISTANCE_SPACING"] = 5 if hdp_active else 1 if cruise_enabled else 0
values["TARGET"] = 1 if hud_control.leadVisible and cruise_enabled else 0
values["TARGET_DISTANCE"] = int(hud_control.leadDistance)
values["BACKGROUND"] = 6 if CS.paddle_button_prev > 0 else 1 if cruise_enabled else 3 if lat_active else 7
values["CENTERLINE"] = 1 if HDA_CntrlModSta > 0 else 0
values["CAR_CIRCLE"] = 2 if hdp_active else 1 if cruise_enabled else 0
values["NAV_ICON"] = 2 if nav_active and cruise_enabled else 1 if main_enabled and nav_active else 0
values["HDA_ICON"] = 5 if hdp_active else 2 if cruise_enabled else 1 if main_enabled else 0
values["LFA_ICON"] = 5 if hdp_active else 2 if lat_active else 1 if lat_enabled else 0
values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 0
values["FCA_ALT_ICON"] = 0
if values["ALERTS_2"] in [1, 2, 5, 6, 10, 21, 22]:
values["ALERTS_2"] = 0
values["DAW_ICON"] = 0
if values["ALERTS_1"] == 0: # alerts가 있으면 사운드도 같이 나옴
values["SOUNDS_1"] = 0
values["SOUNDS_2"] = 0
values["SOUNDS_4"] = 0
if values["ALERTS_3"] in [3, 4, 11, 12, 13, 14, 17, 19, 20, 26, 27, 28, 7, 8, 9, 10]: # hide gap distance msg.(11,12,13,14), lanechange(19,20,27, 28)
values["ALERTS_3"] = 0
values["SOUNDS_3"] = 0
if values["ALERTS_5"] in [1, 2, 3, 4, 5]:
values["ALERTS_5"] = 0
if values["ALERTS_5"] in [11] and CS.softHoldActive == 0:
values["ALERTS_5"] = 0
# curvature 표시(0x161쪽 기존 로직 유지)
_suppress_trailer_mode_warning(values, CS)
curvature = round(CS.out.steeringAngleDeg / 3)
values["LANELINE_CURVATURE"] = (min(abs(curvature), 15) + (-1 if curvature < 0 else 0)) if lat_active else 0
values["LANELINE_CURVATURE_DIRECTION"] = 1 if curvature < 0 and lat_active else 0
trailer_lane_change_blocked = CS.trailer_connected
if trailer_lane_change_blocked:
values["LANELINE_LEFT"] = 2 if hud_control.leftLaneVisible else 0
values["LANELINE_RIGHT"] = 2 if hud_control.rightLaneVisible else 0
else:
lane_color = 6 if md is not None and md.meta.laneChangeAvailableLeft else 2
if lane_line_check >= 1:
lane_line_warn_left = CS.out.leftLaneLine % 10 not in (0, 5)
else:
lane_line_warn_left = CS.out.leftLaneLine // 10 == 2
lane_color = 4 if lane_line_warn_left or CS.out.leftBlindspot else lane_color
if hud_control.leftLaneDepart:
values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1
else:
values["LANELINE_LEFT"] = lane_color if hud_control.leftLaneVisible else 0
lane_color = 6 if md is not None and md.meta.laneChangeAvailableRight else 2
if lane_line_check >= 1:
lane_line_warn_right = CS.out.rightLaneLine % 10 not in (0, 5)
else:
lane_line_warn_right = CS.out.rightLaneLine // 10 == 2
lane_color = 4 if lane_line_warn_right or CS.out.rightBlindspot else lane_color
if hud_control.rightLaneDepart:
values["LANELINE_RIGHT"] = 4 if (frame // 50) % 2 == 0 else 1
else:
values["LANELINE_RIGHT"] = lane_color if hud_control.rightLaneVisible else 0
values["LCA_LEFT_ARROW"] = 2 if CS.out.leftBlinker else 0
values["LCA_RIGHT_ARROW"] = 2 if CS.out.rightBlinker else 0
if trailer_lane_change_blocked:
values["LCA_LEFT_ICON"] = 1 if lat_active else 0
values["LCA_RIGHT_ICON"] = 1 if lat_active else 0
else:
values["LCA_LEFT_ICON"] = (1 if CS.out.leftBlindspot else 2) if lat_active else 0
values["LCA_RIGHT_ICON"] = (1 if CS.out.rightBlindspot else 2) if lat_active else 0
values["LANE_LEFT"] = 0 if trailer_lane_change_blocked else 1 if desire in (1, 3) else 0
values["LANE_RIGHT"] = 0 if trailer_lane_change_blocked else 1 if desire in (2, 4) else 0
ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values, rx_counter = rx_counter))
if CS.adrv_0x200 is not None:
values = copy.copy(CS.adrv_0x200)
rx_counter = values.pop("COUNTER", None)
values["TauGapSet"] = hud_control.leadDistanceBars
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values, rx_counter = rx_counter))
if CS.adrv_0x1ea is not None:
values = copy.copy(CS.adrv_0x1ea)
rx_counter = values.pop("COUNTER", None)
# blinker hold
values['LEFT_BLINK_HOLD'] = 1 if lane_changing == 3 else 0
values['RIGHT_BLINK_HOLD'] = 1 if lane_changing == 4 else 0
_make_ccnc_values(
values, CS, lat_active, frame, hud_control,
lane_line=True,
corner_radar=True,
desire=desire,
# 기존대로 LR/RR만 깜빡임
blink_pairs=[('LR_DETECT', 'LR_DETECT_DISTANCE'),
('RR_DETECT', 'RR_DETECT_DISTANCE')],
blink_t=1.0
)
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values, rx_counter = rx_counter))
if CS.ccnc_0x162 is not None:
values = copy.copy(CS.ccnc_0x162)
if hud_control.leadDistance > 0:
values["FF_DISTANCE"] = hud_control.leadDistance
ff_type = 3 if hud_control.leadRadar == 1 else 13
values["FF_DETECT"] = ff_type if hud_control.leadRelSpeed > -0.1 else ff_type + 1
_make_ccnc_values(
values, CS, lat_active, frame, hud_control,
lane_line=False,
corner_radar=True,
desire=0,
# 필요하면 162도 깜빡임 적용(원래 코드처럼 LR/RR만)
blink_pairs=[('LR_DETECT', 'LR_DETECT_DISTANCE'),
('RR_DETECT', 'RR_DETECT_DISTANCE')],
blink_t=1.0
)
if (left_lane_warning and not CS.out.leftBlinker) or (right_lane_warning and not CS.out.rightBlinker):
values["VIBRATE"] = 1
if canfd_debug > 0:
values["FAULT_LSS"] = 0
values["FAULT_DAS"] = 0
ret.append(packer.make_can_msg("CCNC_0x162", CAN.ECAN, values))
# --- NEW_MSG_4B9 (corner radar keep-alive?) ---
if enable_corner_radar > 0:
if HDA_CntrlModSta == 0:
if frame % 500 in [10, 20, 30]:
values = {
'BYTE_1': 0,
'BYTE_2': 0,
'BYTE_3': 0x80,
'BYTE_4': 0x8A,
'BYTE_5': 0x32,
'BYTE_6': 0x30,
'BYTE_7': 0x01,
'BYTE_8': 0x00,
}
ret.append(packer.make_can_msg("NEW_MSG_4B9", CAN.CAM, values))
elif frame % 500 in [40, 50, 60]:
values = {
'BYTE_1': 0xff,
'BYTE_2': 0xff,
'BYTE_3': 0xff,
'BYTE_4': 0xff,
'BYTE_5': 0xff,
'BYTE_6': 0xff,
'BYTE_7': 0xff,
'BYTE_8': 0xff,
}
ret.append(packer.make_can_msg("NEW_MSG_4B9", CAN.CAM, values))
if False: # canfd_debug > 1 and frame % 20 == 0:
if CS.hda_info_4a3 is not None:
values = copy.copy(CS.hda_info_4a3)
values["LinkClass"] = 1
values["SPEED_LIMIT"] = 100
ret.append(packer.make_can_msg("HDA_INFO_4A3", CAN.CAM, values))
return ret

315
artifacts/package_runtime/iqdbc/car/hyundai/interface.py Normal file
View File

@@ -0,0 +1,315 @@
from iqdbc.car import Bus, get_safety_config, structs
from iqdbc.car.hyundai.hyundaicanfd import CanBus
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ, CAR, DBC, CANFD_RADAR_SCC_CAR, \
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, \
UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, HyundaiSafetyFlagsIQ, HyundaiExtFlags, \
CANFD_HYBRID_STATUS_ADDR, CANFD_HYBRID_STATUS_DLC, \
EV_MODE_STATUS_ADDR, EV_MODE_STATUS_DLC
from iqdbc.car.hyundai.radar_interface import RADAR_START_ADDR
from iqdbc.car.interfaces import CarInterfaceBase
from iqdbc.car.disable_ecu import disable_ecu
from iqdbc.car.hyundai.carcontroller import CarController
from iqdbc.car.hyundai.carstate import CarState
from iqdbc.car.hyundai.radar_interface import RadarInterface
from iqpilot.common.params import Params
ButtonType = structs.CarState.ButtonEvent.Type
Ecu = structs.CarParams.Ecu
# Cancel button can sometimes be ACC pause/resume button, main button can also enable on some cars
ENABLE_BUTTONS = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.cancel, ButtonType.mainCruise)
SteerControlType = structs.CarParams.SteerControlType
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
params = Params()
camera_scc = params.get_int("HyundaiCameraSCC")
if camera_scc > 0:
ret.flags |= HyundaiFlags.CAMERA_SCC.value
print("$$$CAMERA_SCC toggled...")
ret.brand = "hyundai"
if candidate == CAR.KIA_SORENTO:
ret.extFlags |= HyundaiExtFlags.RADAR_GROUP4.value
cam_can = CanBus(None, fingerprint).CAM if camera_scc == 0 else 1
hda2 = 0x50 in fingerprint[cam_can] or 0x110 in fingerprint[cam_can] or params.get_int("CanfdHDA2") > 0
CAN = CanBus(None, fingerprint, hda2)
if ret.flags & HyundaiFlags.CANFD:
# Shared configuration for CAN-FD cars
ret.alphaLongitudinalAvailable = True #candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR)
#ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN]
ret.enableBsm = 0x1ba in fingerprint[CAN.ECAN] # BLINDSPOTS_REAR_CORNERS 0x1ba(442)
if 0x105 in fingerprint[CAN.ECAN]:
ret.flags |= HyundaiFlags.HYBRID.value
# Keep drivetrain/safety classification unchanged: this capability is display-only and requires both exact ECAN frames.
has_ev_mode_status = fingerprint[CAN.ECAN].get(CANFD_HYBRID_STATUS_ADDR) == CANFD_HYBRID_STATUS_DLC and \
fingerprint[CAN.ECAN].get(EV_MODE_STATUS_ADDR) == EV_MODE_STATUS_DLC
if has_ev_mode_status:
ret.extFlags |= HyundaiExtFlags.EV_MODE_STATUS_230.value
if 203 in fingerprint[CAN.CAM]: # LFA_ALT
print("##### Anglecontrol detected (LFA_ALT)")
ret.flags |= HyundaiFlags.ANGLE_CONTROL.value
print("ACAN=", fingerprint[CAN.ACAN])
if 0x210 in fingerprint[CAN.ACAN]:
print("##### Radar Group 1 detected (0x210)")
ret.extFlags |= HyundaiExtFlags.RADAR_GROUP1.value
elif 0x400 in fingerprint[CAN.ACAN] and 0x41D in fingerprint[CAN.ACAN]:
print("##### Radar Group 3 detected (0x400-0x41D)")
ret.extFlags |= HyundaiExtFlags.RADAR_GROUP3.value
if all(fingerprint[CAN.ACAN].get(addr) == 32 for addr in range(0x235, 0x249)):
ret.extFlags |= HyundaiExtFlags.CORNER_RADAR_OBJECTS_235.value
print("##### Corner radar objects 0x235 group detected")
if all(fingerprint[CAN.ACAN].get(addr) == 32 for addr in range(0x180, 0x185)):
ret.extFlags |= HyundaiExtFlags.CORNER_RADAR_OBJECTS_180.value
print("##### Corner radar objects 0x180 group detected")
if all(fingerprint[CAN.ACAN].get(addr) == 32 for addr in tuple(range(0x430, 0x438)) + tuple(range(0x440, 0x448))):
ret.extFlags |= HyundaiExtFlags.CORNER_RADAR_OBJECTS_430.value
print("##### Corner radar objects 0x430/0x440 group detected")
# detect HDA2 with ADAS Driving ECU
if hda2:
print("$$$CANFD HDA2")
ret.flags |= HyundaiFlags.CANFD_HDA2.value
if camera_scc > 0:
if 0x110 in fingerprint[CAN.ACAN]:
ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
print("$$$CANFD ALT_STEERING1")
else:
if 0x110 in fingerprint[CAN.CAM]: # 0x110(272): LKAS_ALT
ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
print("$$$CANFD ALT_STEERING1")
## carrot_todo: sorento:
if 0x2a4 not in fingerprint[CAN.CAM]: # 0x2a4(676): CAM_0x2a4
ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
print("$$$CANFD ALT_STEERING2")
## carrot: canival 4th, no 0x1cf
if 0x1cf not in fingerprint[CAN.ECAN]: # 0x1cf(463): CRUISE_BUTTONS
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
print("$$$CANFD ALT_BUTTONS")
else:
# non-HDA2
print("$$$CANFD non HDA2")
if 0x1cf not in fingerprint[CAN.ECAN]:
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
print("$$$CANFD ALT_BUTTONS")
if not ret.flags & HyundaiFlags.RADAR_SCC:
ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value
#if not ret.flags & HyundaiFlags.RADAR_SCC:
# ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value
# print("$$$CANFD CAMERA_SCC")
# Some HDA2 cars have alternative messages for gear checks
# ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead
if 0x40 in fingerprint[CAN.ECAN]: # 0x40(64): GEAR_ALT
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value
print("$$$CANFD ALT_GEARS")
elif 69 in fingerprint[CAN.ECAN]: # Special case
ret.extFlags |= HyundaiExtFlags.CANFD_GEARS_69.value
print("$$$CANFD GEARS_69")
elif 112 in fingerprint[CAN.ECAN]: # carrot: eGV70
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS_2.value
print("$$$CANFD ALT_GEARS_2")
elif 0x130 in fingerprint[CAN.ECAN]: # 0x130(304): GEAR_SHIFTER
print("$$$CANFD GEAR_SHIFTER present")
else:
ret.extFlags |= HyundaiExtFlags.CANFD_GEARS_NONE.value
print("$$$CANFD GEARS_NONE")
cfgs = [get_safety_config(structs.CarParams.SafetyModel.hyundaiCanfd), ]
if CAN.ECAN >= 4:
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
ret.safetyConfigs = cfgs
if ret.flags & HyundaiFlags.CANFD_HDA2:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_LKA_STEERING.value
if ret.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT.value
if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ALT_BUTTONS.value
if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAMERA_SCC.value
else:
# Shared configuration for non CAN-FD cars
ret.alphaLongitudinalAvailable = True #candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR)
ret.enableBsm = 0x58b in fingerprint[0]
print(f"$$$ enableBsm = {ret.enableBsm}")
# Send LFA message on cars with HDA
if 0x485 in fingerprint[2]:
ret.flags |= HyundaiFlags.SEND_LFA.value
print("$$$SEND_LFA")
# These cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
if 0x38d in fingerprint[0] or 0x38d in fingerprint[2]:
ret.flags |= HyundaiFlags.USE_FCA.value
print("$$$USE_FCA")
if ret.flags & HyundaiFlags.LEGACY:
# these cars require a special panda safety mode due to missing counters and checksums in the messages
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hyundaiLegacy)]
print("$$$Legacy Safety Model")
else:
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hyundai, 0)]
if ret.flags & HyundaiFlags.CAMERA_SCC:
ret.safetyConfigs[0].safetyParam |= HyundaiSafetyFlags.CAMERA_SCC.value
print("$$$CAMERA_SCC")
# Common lateral control setup
ret.centerToFront = ret.wheelbase * 0.4
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.4
if ret.flags & HyundaiFlags.ANGLE_CONTROL:
ret.steerControlType = SteerControlType.angle
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if ret.flags & HyundaiFlags.ALT_LIMITS:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.ALT_LIMITS.value
# Common longitudinal control setup
ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or Bus.radar not in DBC[ret.carFingerprint]
ret.openpilotLongitudinalControl = alpha_long and ret.alphaLongitudinalAvailable
# carrot, if camera_scc enabled, enable openpilotLongitudinalControl
enable_radar_tracks = params.get_int("EnableRadarTracks")
if ret.flags & HyundaiFlags.CAMERA_SCC.value or enable_radar_tracks > 0 or enable_radar_tracks == -2:
ret.radarUnavailable = False
ret.openpilotLongitudinalControl = True if camera_scc < 3 else False
print(f"$$$OenpilotLongitudinalControl = True, CAMERA_SCC({ret.flags & HyundaiFlags.CAMERA_SCC.value}) or RadarTracks{enable_radar_tracks}")
else:
print(f"$$$OenpilotLongitudinalControl = {alpha_long}")
#ret.radarUnavailable = False # TODO: canfd... carrot, hyundai cars have radar
ret.radarTimeStep = 0.05 #if params.get_int("EnableRadarTracks") > 0 else 0.02
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.startingState = False # True # carrot
ret.vEgoStarting = 0.1
ret.startAccel = 1.0
ret.longitudinalActuatorDelay = 0.5
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [1.]
ret.longitudinalTuning.kf = 1.0
# *** feature detection ***
if ret.flags & HyundaiFlags.CANFD:
print(f"$$$$$ CanFD ECAN = {CAN.ECAN}")
if 0x1fa in fingerprint[CAN.ECAN]:
ret.extFlags |= HyundaiExtFlags.NAVI_CLUSTER.value
print("$$$$ NaviCluster = True")
else:
print("$$$$ NaviCluster = False")
else:
if 1348 in fingerprint[0]:
ret.extFlags |= HyundaiExtFlags.NAVI_CLUSTER.value
print("$$$$ NaviCluster = True")
if 1157 in fingerprint[0] or 1157 in fingerprint[2]:
ret.extFlags |= HyundaiExtFlags.HAS_LFAHDA.value
print("$$$$ HasLFAHDA")
if 1007 in fingerprint[0]:
print("#### cruiseButtonAlt")
print(f"$$$$ enableBsm = {ret.enableBsm}")
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.LONG.value
if ret.flags & HyundaiFlags.HYBRID:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.HYBRID_GAS.value
elif ret.flags & HyundaiFlags.EV:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.EV_GAS.value
elif ret.flags & HyundaiFlags.FCEV:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.FCEV_GAS.value
# Car specific configuration overrides
if candidate == CAR.KIA_OPTIMA_G4_FL:
ret.steerActuatorDelay = 0.2
# Dashcam cars are missing a test route, or otherwise need validation
# TODO: Optima Hybrid 2017 uses a different SCC12 checksum
#ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, }
return ret
@staticmethod
def _get_params_iq(stock_cp, ret, candidate, fingerprint, car_fw, alpha_long, is_release_iq, docs):
del candidate, car_fw, alpha_long, is_release_iq, docs
if not stock_cp.flags & HyundaiFlags.CANFD and 0x391 in fingerprint[0]:
ret.flags |= HyundaiFlagsIQ.HAS_LFA_BUTTON
ret.iqSafetyFlags |= HyundaiSafetyFlagsIQ.HAS_LDA_BUTTON
return ret
@staticmethod
def init(CP, CP_IQ, can_recv, can_send):
del CP_IQ
Params().put_int('LongitudinalPersonalityMax', 4)
if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC):
addr, bus = 0x7d0, 0
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, CanBus(CP).ECAN
disable_ecu(can_recv, can_send, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
params = Params()
if params.get_int("EnableRadarTracks") > 0 and not CP.flags & HyundaiFlags.CANFD:
result = enable_radar_tracks(CP, can_recv, can_send)
params.put_bool("EnableRadarTracksResult", result)
# for blinkers
if CP.flags & HyundaiFlags.ENABLE_BLINKERS:
disable_ecu(can_recv, can_send, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01')
def enable_radar_tracks(CP, logcan, sendcan):
from iqdbc.car.isotp_parallel_query import IsoTpParallelQuery
print("################ Try To Enable Radar Tracks ####################")
ret = False
sccBus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC.value else 0
rdr_fw = None
rdr_fw_address = 0x7d0 #
try:
try:
query = IsoTpParallelQuery(sendcan, logcan, sccBus, [rdr_fw_address], [b'\x10\x07'], [b'\x50\x07'])
for addr, dat in query.get_data(0.1).items(): # pylint: disable=unused-variable
print("ecu write data by id ...")
new_config = b"\x00\x00\x00\x01\x00\x01"
#new_config = b"\x00\x00\x00\x00\x00\x01"
dataId = b'\x01\x42'
WRITE_DAT_REQUEST = b'\x2e'
WRITE_DAT_RESPONSE = b'\x68'
query = IsoTpParallelQuery(sendcan, logcan, sccBus, [rdr_fw_address], [WRITE_DAT_REQUEST+dataId+new_config], [WRITE_DAT_RESPONSE])
result = query.get_data(0)
print("result=", result)
ret = True
break
except Exception as e:
print(f"Failed : {e}")
except Exception as e:
print("############## Failed to enable tracks" + str(e))
print("################ END Try to enable radar tracks")
return ret

885
artifacts/package_runtime/iqdbc/car/hyundai/radar_interface.py Normal file
View File

@@ -0,0 +1,885 @@
import math
import os
from collections import deque
from iqdbc import DBC_PATH
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.interfaces import RadarInterfaceBase
from iqdbc.car.hyundai.values import DBC, HyundaiFlags, HyundaiExtFlags
from iqpilot.common.params import Params
from iqdbc.car.hyundai.hyundaicanfd import CanBus
from iqpilot.common.filter_simple import MyMovingAverage
SCC_TID = 0
RADAR_START_ADDR = 0x500
RADAR_MSG_COUNT = 32
RADAR_MSG_COUNT4 = 8
RADAR_GROUP4_MAX_LONG_DIST = 325.0
RADAR_GROUP4_MAX_YREL = 6.0
RADAR_START_ADDR_CANFD1 = 0x210
RADAR_MSG_COUNT1 = 16
RADAR_START_ADDR_CANFD2 = 0x3A5 # Group 2, Group 1: 0x210 2개씩?어???단 보류.
RADAR_MSG_COUNT2 = 32
RADAR_START_ADDR_CANFD3 = 0x400
RADAR_MSG_COUNT3 = 30
CORNER_OBJECT_235_START_ADDR = 0x235
CORNER_OBJECT_235_MSG_COUNT = 20
CORNER_OBJECT_235_TRACK_ID_OFFSET = 200
CORNER_OBJECT_235_DBC = 'hyundai_canfd_corner_radar_235_generated'
CORNER_OBJECT_180_START_ADDR = 0x180
CORNER_OBJECT_180_MSG_COUNT = 5
CORNER_OBJECT_180_SLOTS_PER_MSG = 2
CORNER_OBJECT_180_TRACK_ID_OFFSET = 240
CORNER_OBJECT_180_DBC = 'hyundai_canfd_corner_radar_180_generated'
CORNER_OBJECT_430_LEFT_START_ADDR = 0x430
CORNER_OBJECT_430_RIGHT_START_ADDR = 0x440
CORNER_OBJECT_430_MSG_COUNT_PER_SIDE = 8
CORNER_OBJECT_430_SLOTS_PER_MSG = 7
CORNER_OBJECT_430_TRACK_ID_OFFSET = 300
CORNER_OBJECT_430_DBC = 'hyundai_canfd_corner_radar_430_generated'
CORNER_OBJECT_430_EMPTY_RAW_VALUES = (0x010d1f40, 0x00010d1f)
CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MIN = 2520 # 126.0 m
CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MAX = 2600 # 130.0 m
CORNER_OBJECT_430_MAX_DREL = 120.0
CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE = 4
CORNER_OBJECT_430_DT = 0.05
CORNER_OBJECT_430_MAX_DREL_DELTA = 1.5
CORNER_OBJECT_430_CANDIDATE_META_BYTE_3 = (2,)
CORNER_OBJECT_430_CANDIDATE_EXCLUDED_SLOTS = (1,)
CORNER_OBJECT_430_CANDIDATE_RAW_DELTA = 200
CORNER_OBJECT_430_STRONG_META_BYTE_2 = (10,)
CORNER_OBJECT_430_WEAK_META_BYTE_2 = (5, 6, 7, 8, 9)
CORNER_OBJECT_430_STRONG_MIN_SUPPORT = 2
CORNER_OBJECT_430_WEAK_MIN_SUPPORT = 3
CORNER_OBJECT_430_CLUSTER_RAW_GAP = 200
CORNER_OBJECT_430_TRACK_MATCH_MAX_DREL_DELTA = 3.0
CORNER_OBJECT_430_MAX_ABS_VREL = 20.0
CORNER_OBJECT_430_MAX_ABS_YVREL = 3.0
CORNER_OBJECT_430_VREL_ALPHA = 0.35
CORNER_OBJECT_430_YVREL_ALPHA = 0.35
CORNER_OBJECT_430_LATERAL_CELL_MSG_WEIGHT = 0.35
CORNER_OBJECT_430_LATERAL_CELL_SLOT_WEIGHT = 0.65
CORNER_OBJECT_430_YREL_OFFSET = 5.8
CORNER_OBJECT_430_YREL_SCALE = 1.1
CORNER_OBJECT_430_RIGHT_CELL_MIRROR = 7.0
CORNER_OBJECT_430_MIN_ABS_YREL = 0.8
CORNER_OBJECT_430_MAX_ABS_YREL = 4.2
CORNER_OBJECT_430_HISTORY_SIZE = 8
CORNER_OBJECT_430_MIN_HISTORY = 5
CORNER_OBJECT_430_MIN_INWARD_YREL_DELTA = 0.35
CORNER_OBJECT_430_MIN_RECENT_INWARD_YREL_DELTA = 0.05
CORNER_OBJECT_430_MIN_INWARD_RATIO = 0.65
CORNER_OBJECT_430_INWARD_CENTER_ABS_YREL = 1.55
CORNER_OBJECT_430_INWARD_KEEP_YVREL_ABS_YREL = 2.2
CORNER_OBJECT_430_EARLY_INWARD_NONCENTER_FRAMES = 2
CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL = 2.0
CORNER_OBJECT_STABLE_TRACK_ID_START = 1000
CORNER_SIDE_OBJECT_MAX_DREL = 0.2
CORNER_SIDE_OBJECT_MIN_ABS_YREL = 1.4
CORNER_SIDE_OBJECT_MAX_ABS_YREL = 4.5
# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
class CornerObjectTrackIdManager:
def __init__(self):
self.next_track_id = CORNER_OBJECT_STABLE_TRACK_ID_START
self.objects: dict[tuple[str, int], tuple[int, int]] = {}
def get_track_id(self, source: str, object_id: int, age: int) -> int:
key = (source, object_id)
previous = self.objects.get(key)
if previous is None or age < previous[1]:
track_id = self.next_track_id
self.next_track_id += 1
else:
track_id = previous[0]
self.objects[key] = (track_id, age)
return track_id
def clear_source(self, source: str):
self.objects = {key: value for key, value in self.objects.items() if key[0] != source}
def corner_object_position_valid(d_rel: float, y_rel: float) -> bool:
normal_object = 0.2 < d_rel < 180.0
clipped_side_object = (
0.0 <= d_rel <= CORNER_SIDE_OBJECT_MAX_DREL and
CORNER_SIDE_OBJECT_MIN_ABS_YREL <= abs(y_rel) <= CORNER_SIDE_OBJECT_MAX_ABS_YREL
)
return (normal_object or clipped_side_object) and abs(y_rel) < 40.0
def get_radar_can_parser(CP, radar_tracks, msg_start_addr, msg_count, radar_group4=False):
if not radar_tracks:
return None
#if Bus.radar not in DBC[CP.carFingerprint]:
# return None
print("RadarInterface: RadarTracks...")
if CP.flags & HyundaiFlags.CANFD:
CAN = CanBus(CP)
messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(msg_start_addr, msg_start_addr + msg_count)]
return CANParser('hyundai_canfd_radar_generated', messages, CAN.ACAN)
else:
messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(msg_start_addr, msg_start_addr + msg_count)]
#return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 1)
dbc_name = 'hyundai_kia_denso_front_radar_generated' if radar_group4 else 'hyundai_kia_mando_front_radar_generated'
return CANParser(dbc_name, messages, 1)
def get_corner_object_can_parser(CP, enabled):
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
return None
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_235_DBC}.dbc")
if not os.path.exists(dbc_path):
print(f"RadarInterface: missing {CORNER_OBJECT_235_DBC}.dbc, 0x235 corner radar disabled")
return None
CAN = CanBus(CP)
messages = [(f"CORNER_RADAR_235_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_235_START_ADDR, CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT)]
return CANParser(CORNER_OBJECT_235_DBC, messages, CAN.ACAN)
def get_corner_object_180_can_parser(CP, enabled):
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
return None
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_180_DBC}.dbc")
if not os.path.exists(dbc_path):
print(f"RadarInterface: missing {CORNER_OBJECT_180_DBC}.dbc, 0x180 corner radar disabled")
return None
CAN = CanBus(CP)
messages = [(f"CORNER_RADAR_180_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_180_START_ADDR, CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT)]
return CANParser(CORNER_OBJECT_180_DBC, messages, CAN.ACAN)
def get_corner_object_430_can_parser(CP, enabled):
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
return None
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_430_DBC}.dbc")
if not os.path.exists(dbc_path):
print(f"RadarInterface: missing {CORNER_OBJECT_430_DBC}.dbc, 0x430/0x440 corner radar disabled")
return None
CAN = CanBus(CP)
messages = [(f"CORNER_RADAR_430_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_430_LEFT_START_ADDR, CORNER_OBJECT_430_LEFT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)]
messages += [(f"CORNER_RADAR_430_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_430_RIGHT_START_ADDR, CORNER_OBJECT_430_RIGHT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)]
return CANParser(CORNER_OBJECT_430_DBC, messages, CAN.ACAN)
def get_radar_can_parser_scc(CP):
CAN = CanBus(CP)
if CP.flags & HyundaiFlags.CANFD:
messages = [("SCC_CONTROL", 50)]
bus = CAN.ECAN
else:
messages = [("SCC11", 50)]
bus = CAN.ECAN
print("$$$$$$$$ ECAN = ", CAN.ECAN)
bus = CAN.CAM if CP.flags & HyundaiFlags.CAMERA_SCC else bus
return CANParser(DBC[CP.carFingerprint][Bus.pt], messages, bus)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP, CP_IQ=None):
super().__init__(CP, CP_IQ or structs.IQCarParams())
self.v_ego = 0.0
self.canfd = True if CP.flags & HyundaiFlags.CANFD else False
self.radar_group1 = False
self.radar_group3 = False
self.radar_group4 = not self.canfd and bool(CP.extFlags & HyundaiExtFlags.RADAR_GROUP4.value)
if self.canfd:
if CP.extFlags & HyundaiExtFlags.RADAR_GROUP1.value:
self.radar_start_addr = RADAR_START_ADDR_CANFD1
self.radar_msg_count = RADAR_MSG_COUNT1
self.radar_group1 = True
elif CP.extFlags & HyundaiExtFlags.RADAR_GROUP3.value:
self.radar_start_addr = RADAR_START_ADDR_CANFD3
self.radar_msg_count = RADAR_MSG_COUNT3
self.radar_group3 = True
else:
self.radar_start_addr = RADAR_START_ADDR_CANFD2
self.radar_msg_count = RADAR_MSG_COUNT2
else:
self.radar_start_addr = RADAR_START_ADDR
self.radar_msg_count = RADAR_MSG_COUNT4 if self.radar_group4 else RADAR_MSG_COUNT
self.params = Params()
self.radar_tracks = self.params.get_int("EnableRadarTracks") >= 1
self.corner_object_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_235.value) and self.params.get_int("EnableCornerRadar") > 0
self.corner_object_180_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_180.value) and self.params.get_int("EnableCornerRadar") > 0
self.corner_object_430_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_430.value) and self.params.get_int("EnableCornerRadar") > 0
self.updated_tracks = set()
self.updated_scc = set()
self.updated_corner_objects = set()
self.updated_corner_objects_180 = set()
self.updated_corner_objects_430 = set()
self.corner_object_missed_updates = 0
self.corner_object_180_missed_updates = 0
self.corner_object_430_missed_updates = 0
self.corner_object_track_ids = CornerObjectTrackIdManager()
self.rcp_tracks = get_radar_can_parser(CP, self.radar_tracks, self.radar_start_addr, self.radar_msg_count, self.radar_group4)
self.rcp_corner_objects = get_corner_object_can_parser(CP, self.corner_object_tracks)
self.rcp_corner_objects_180 = get_corner_object_180_can_parser(CP, self.corner_object_180_tracks)
self.rcp_corner_objects_430 = get_corner_object_430_can_parser(CP, self.corner_object_430_tracks)
# Enabling raw radar tracks on legacy CAN disables the stock SCC11 stream on
# some Hyundai/Kia platforms. Camera-SCC cars may still use SCC11.
use_scc_parser = not (self.radar_tracks and not self.canfd and not (CP.flags & HyundaiFlags.CAMERA_SCC))
self.rcp_scc = get_radar_can_parser_scc(CP) if use_scc_parser else None
self.trigger_msg_scc = 416 if self.canfd else 0x420
self.trigger_msg_tracks = self.radar_start_addr + self.radar_msg_count - 1
self.trigger_msg_corner_objects = CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT - 1
self.trigger_msg_corner_objects_180 = CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT - 1
self.trigger_msg_corner_objects_430 = CORNER_OBJECT_430_RIGHT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE - 1
self.track_id = 0
self.corner_objects_available = self.rcp_corner_objects is not None or self.rcp_corner_objects_180 is not None or self.rcp_corner_objects_430 is not None
self.radar_off_can = CP.radarUnavailable and not self.corner_objects_available
print(
"RadarInterface: "
f"radarUnavailable={CP.radarUnavailable} radarTracks={self.radar_tracks} "
f"group4={self.radar_group4} "
f"corner235={self.rcp_corner_objects is not None} corner180={self.rcp_corner_objects_180 is not None} "
f"corner430={self.rcp_corner_objects_430 is not None} "
f"radarOffCan={self.radar_off_can}"
)
self.vRel_last = 0
self.dRel_last = 0
self.corner_object_430_prev_d_rel = {}
self.corner_object_430_prev_v_rel = {}
self.corner_object_430_prev_y_rel = {}
self.corner_object_430_prev_yv_rel = {}
self.corner_object_430_prev_code = {}
self.corner_object_430_history = {}
self.corner_object_430_noncenter_inward_frames = {}
# Initialize pts
if self.rcp_tracks is not None:
total_tracks = self.radar_msg_count * (2 if self.radar_group1 else 1)
for track_id in range(total_tracks):
t_id = track_id + 32
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
if self.rcp_scc is not None:
self.pts[SCC_TID] = structs.RadarData.RadarPoint()
self.pts[SCC_TID].trackId = SCC_TID
self.pts[SCC_TID].radarSource = "scc"
if self.rcp_corner_objects is not None:
for slot in range(CORNER_OBJECT_235_MSG_COUNT):
t_id = CORNER_OBJECT_235_TRACK_ID_OFFSET + slot
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
self.pts[t_id].radarSource = "corner235"
if self.rcp_corner_objects_180 is not None:
for slot in range(CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG):
t_id = CORNER_OBJECT_180_TRACK_ID_OFFSET + slot
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
self.pts[t_id].radarSource = "corner180"
if self.rcp_corner_objects_430 is not None:
for slot in range(CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * 2 * CORNER_OBJECT_430_SLOTS_PER_MSG):
t_id = CORNER_OBJECT_430_TRACK_ID_OFFSET + slot
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
self.frame = 0
def update(self, can_strings):
self.frame += 1
if self.radar_off_can or (self.rcp_tracks is None and self.rcp_scc is None and self.rcp_corner_objects is None and self.rcp_corner_objects_180 is None and self.rcp_corner_objects_430 is None):
return super().update(None)
if self.rcp_scc is not None:
vls_s = self.rcp_scc.update(can_strings)
self.updated_scc.update(vls_s)
track_ready = False
if self.radar_tracks and self.rcp_tracks is not None:
vls_t = self.rcp_tracks.update(can_strings)
self.updated_tracks.update(vls_t)
track_ready = self.trigger_msg_tracks in self.updated_tracks
corner_ready = False
if self.rcp_corner_objects is not None:
vls_c = self.rcp_corner_objects.update(can_strings)
self.updated_corner_objects.update(vls_c)
corner_ready = self.trigger_msg_corner_objects in self.updated_corner_objects
corner_180_ready = False
if self.rcp_corner_objects_180 is not None:
vls_180 = self.rcp_corner_objects_180.update(can_strings)
self.updated_corner_objects_180.update(vls_180)
corner_180_ready = self.trigger_msg_corner_objects_180 in self.updated_corner_objects_180
corner_430_ready = False
if self.rcp_corner_objects_430 is not None:
vls_430 = self.rcp_corner_objects_430.update(can_strings)
self.updated_corner_objects_430.update(vls_430)
corner_430_ready = self.trigger_msg_corner_objects_430 in self.updated_corner_objects_430
scc_ready = not self.radar_tracks and self.frame % 5 == 0 and self.rcp_scc is not None
if track_ready:
self._update(self.updated_tracks)
self.updated_tracks.clear()
if corner_ready:
self._update_corner_objects(self.updated_corner_objects)
self.corner_object_missed_updates = 0
self.updated_corner_objects.clear()
if corner_180_ready:
self._update_corner_objects_180(self.updated_corner_objects_180)
self.corner_object_180_missed_updates = 0
self.updated_corner_objects_180.clear()
if corner_430_ready:
self._update_corner_objects_430(self.updated_corner_objects_430)
self.corner_object_430_missed_updates = 0
self.updated_corner_objects_430.clear()
# Corner radar runs at its own cadence. Do not let corner-only frames publish
# RadarData, since liveTracks uses a fixed radarTimeStep for aLead/jLead.
publish_ready = track_ready or scc_ready
if not publish_ready:
return None
if self.rcp_scc is not None:
self._update_scc(self.updated_scc)
if self.rcp_corner_objects is not None:
if self.updated_corner_objects:
self._update_corner_objects(self.updated_corner_objects)
self.corner_object_missed_updates = 0
else:
self.corner_object_missed_updates += 1
if self.corner_object_missed_updates > 10:
self._clear_corner_objects()
if self.rcp_corner_objects_180 is not None:
if self.updated_corner_objects_180:
self._update_corner_objects_180(self.updated_corner_objects_180)
self.corner_object_180_missed_updates = 0
else:
self.corner_object_180_missed_updates += 1
if self.corner_object_180_missed_updates > 10:
self._clear_corner_objects_180()
if self.rcp_corner_objects_430 is not None:
if self.updated_corner_objects_430:
self._update_corner_objects_430(self.updated_corner_objects_430)
self.corner_object_430_missed_updates = 0
else:
self.corner_object_430_missed_updates += 1
if self.corner_object_430_missed_updates > 10:
self._clear_corner_objects_430()
self.updated_scc.clear()
self.updated_corner_objects.clear()
self.updated_corner_objects_180.clear()
self.updated_corner_objects_430.clear()
ret = structs.RadarData()
if ((self.rcp_tracks is not None and self.radar_tracks and not self.rcp_tracks.can_valid) or
(self.rcp_scc is not None and not self.corner_objects_available and not self.rcp_scc.can_valid) or
(self.rcp_corner_objects is not None and not self.rcp_corner_objects.can_valid) or
(self.rcp_corner_objects_180 is not None and not self.rcp_corner_objects_180.can_valid) or
(self.rcp_corner_objects_430 is not None and not self.rcp_corner_objects_430.can_valid)):
ret.errors.canError = True
ret.points = [point for point in self.pts.values() if point.measured]
return ret
def _update(self, updated_messages):
t_id = 32
for addr in range(self.radar_start_addr, self.radar_start_addr + self.radar_msg_count):
msg = self.rcp_tracks.vl[f"RADAR_TRACK_{addr:x}"]
if self.radar_group1:
valid = msg['VALID_CNT1'] > 10
elif self.radar_group3:
# Group 3 marks an empty object slot with LONG_DIST raw 0x7ff (204.7 m).
valid = msg['LONG_DIST'] < 204.7
elif self.canfd:
valid = msg['VALID_CNT'] > 10
elif self.radar_group4:
# EN: DNMWR006 exposes eight stable tracked-object slots at 0x500-0x507.
# Messages from 0x508 onward are distance-sorted raw detections without
# stable IDs, so they are excluded. OBJECT_STATE 3 is a confirmed track;
# empty slots use LONG_DIST raw 0xfff8 (409.55 m). Driving logs reached
# 317.80 m, so 325 m preserves every observed confirmed track while
# retaining margin from the empty-slot sentinel. Keep the +/-6 m
# ego/adjacent-lane envelope to suppress farther roadside reflections.
# KO: DNMWR006의 안정적인 추적 객체 슬롯은 0x500~0x507의 8개임.
# 0x508 이후 메시지는 고정 ID가 없는 거리순 raw detection이므로 제외함.
# OBJECT_STATE 3은 확정 추적 객체이며, 빈 슬롯은 LONG_DIST raw
# 0xfff8(409.55m)을 사용함. 주행 로그의 최대값은 317.80m였으므로
# 325m 상한으로 관측된 확정 트랙을 모두 보존하면서 빈 슬롯 값과 충분한
# 여유를 확보함. 원거리 도로변 반사를 줄이기 위해 좌우 6m 범위를 유지함.
valid = (msg['OBJECT_STATE'] == 3 and 0.2 < msg['LONG_DIST'] < RADAR_GROUP4_MAX_LONG_DIST and
abs(msg['LAT_DIST']) <= RADAR_GROUP4_MAX_YREL)
else:
valid = msg['STATE'] in (3, 4)
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
elif self.radar_group1:
self.pts[t_id].dRel = msg['LONG_DIST1']
self.pts[t_id].yRel = msg['LAT_DIST1']
self.pts[t_id].vRel = msg['REL_SPEED1']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL1']
self.pts[t_id].yvRel = msg['LAT_SPEED1']
elif self.canfd:
if self.radar_group3:
# Group 3 reports the object's center. Convert it to the rear surface to match SCC/vision dRel.
self.pts[t_id].dRel = max(0.0, msg['LONG_DIST'] - msg['OBJECT_LENGTH'] * 0.5 - 0.1)
else:
self.pts[t_id].dRel = msg['LONG_DIST']
self.pts[t_id].yRel = msg['LAT_DIST']
self.pts[t_id].vRel = msg['REL_SPEED']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan') if self.radar_group3 else msg['REL_ACCEL']
self.pts[t_id].yvRel = 0.0 if self.radar_group3 else msg['LAT_SPEED']
elif self.radar_group4:
self.pts[t_id].dRel = msg['LONG_DIST']
self.pts[t_id].yRel = -msg['LAT_DIST']
self.pts[t_id].vRel = msg['REL_SPEED']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0.0
else:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[t_id].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[t_id].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[t_id].vRel = msg['REL_SPEED']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL']
self.pts[t_id].yvRel = 0.0
t_id += 1
# radar group1? ?나??msg??2개의 ?이?? ?어?음.
if self.radar_group1:
for addr in range(self.radar_start_addr, self.radar_start_addr + self.radar_msg_count):
msg = self.rcp_tracks.vl[f"RADAR_TRACK_{addr:x}"]
valid = msg['VALID_CNT2'] > 10
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = msg['LONG_DIST2']
self.pts[t_id].yRel = msg['LAT_DIST2']
self.pts[t_id].vRel = msg['REL_SPEED2']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL2']
self.pts[t_id].yvRel = msg['LAT_SPEED2']
t_id += 1
def _update_corner_objects(self, updated_messages):
if self.rcp_corner_objects is None:
return
if not updated_messages:
self._clear_corner_objects()
return
candidates = []
for slot, addr in enumerate(range(CORNER_OBJECT_235_START_ADDR, CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT)):
t_id = CORNER_OBJECT_235_TRACK_ID_OFFSET + slot
msg = self.rcp_corner_objects.vl[f"CORNER_RADAR_235_OBJECTS_{addr:x}"]
d_rel = msg["OBJ_REL_POS_X"]
y_rel = msg["OBJ_REL_POS_Y"]
v_rel = msg["OBJ_REL_VEL_X"]
yv_rel = msg["OBJ_REL_VEL_Y"]
a_rel = msg["OBJ_REL_ACCEL_X"]
# Side objects are clipped to x=0 by the corner radar. Quality, identity,
# and lateral motion still describe a real object, so keep them for
# corner-confirmed front-radar association in radard.
valid = msg["OBJ_QUAL_LEVEL"] > 0 and corner_object_position_valid(d_rel, y_rel) and v_rel > -99.0
if not valid:
continue
candidates.append((t_id, int(msg["OBJ_OBJECT_ID"]), int(msg["OBJ_AGE"]), int(msg["OBJ_QUAL_LEVEL"]),
d_rel, y_rel, v_rel, yv_rel, a_rel))
self._apply_corner_objects("corner235", candidates,
range(CORNER_OBJECT_235_TRACK_ID_OFFSET,
CORNER_OBJECT_235_TRACK_ID_OFFSET + CORNER_OBJECT_235_MSG_COUNT))
def _update_corner_objects_180(self, updated_messages):
if self.rcp_corner_objects_180 is None:
return
if not updated_messages:
self._clear_corner_objects_180()
return
candidates = []
for msg_index, addr in enumerate(range(CORNER_OBJECT_180_START_ADDR, CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT)):
msg = self.rcp_corner_objects_180.vl[f"CORNER_RADAR_180_OBJECTS_{addr:x}"]
for slot_index in range(CORNER_OBJECT_180_SLOTS_PER_MSG):
t_id = CORNER_OBJECT_180_TRACK_ID_OFFSET + msg_index * CORNER_OBJECT_180_SLOTS_PER_MSG + slot_index
prefix = f"SLOT{slot_index + 1}_"
d_rel = msg[f"{prefix}REL_POS_X"]
y_rel = msg[f"{prefix}REL_POS_Y"]
v_rel = msg[f"{prefix}REL_VEL_X"]
yv_rel = msg[f"{prefix}REL_VEL_Y"]
a_rel = msg[f"{prefix}REL_ACCEL_X"]
valid = msg[f"{prefix}QUAL_LEVEL"] > 0 and corner_object_position_valid(d_rel, y_rel) and v_rel > -99.0
if not valid:
continue
candidates.append((t_id, int(msg[f"{prefix}OBJECT_ID"]), int(msg[f"{prefix}AGE"]), int(msg[f"{prefix}QUAL_LEVEL"]),
d_rel, y_rel, v_rel, yv_rel, a_rel))
self._apply_corner_objects("corner180", candidates,
range(CORNER_OBJECT_180_TRACK_ID_OFFSET,
CORNER_OBJECT_180_TRACK_ID_OFFSET + CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG))
def _apply_corner_objects(self, source, candidates, slot_ids):
for t_id in slot_ids:
self._clear_point(t_id)
# The same object can occupy two CAN slots for one cycle during a slot handoff.
# Publish only the newest/highest-quality copy so trackId stays unique.
objects = {}
for candidate in candidates:
object_id = candidate[1]
previous = objects.get(object_id)
if previous is None or (candidate[2], candidate[3]) > (previous[2], previous[3]):
objects[object_id] = candidate
for t_id, object_id, age, _, d_rel, y_rel, v_rel, yv_rel, a_rel in objects.values():
point = self.pts[t_id]
point.measured = True
point.trackId = self.corner_object_track_ids.get_track_id(source, object_id, age)
point.radarSource = source
point.dRel = d_rel
point.yRel = y_rel
point.vRel = v_rel
point.vLead = v_rel + self.v_ego
point.aRel = a_rel
point.yvRel = yv_rel
def _update_corner_objects_430(self, updated_messages):
if self.rcp_corner_objects_430 is None:
return
if not updated_messages:
self._clear_corner_objects_430()
return
bank_defs = (
(CORNER_OBJECT_430_LEFT_START_ADDR, 1.0, 0),
(CORNER_OBJECT_430_RIGHT_START_ADDR, -1.0, CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * CORNER_OBJECT_430_SLOTS_PER_MSG),
)
for start_addr, side_sign, track_base in bank_defs:
bins = []
for msg_index, addr in enumerate(range(start_addr, start_addr + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)):
msg = self.rcp_corner_objects_430.vl[f"CORNER_RADAR_430_OBJECTS_{addr:x}"]
for slot_index in range(CORNER_OBJECT_430_SLOTS_PER_MSG):
prefix = f"SLOT{slot_index + 1}_"
distance_raw = int(msg[f"{prefix}DISTANCE_RAW"])
raw = (
distance_raw |
(int(msg[f"{prefix}META_13_15"]) << 13) |
(int(msg[f"{prefix}META_BYTE_2"]) << 16) |
(int(msg[f"{prefix}META_BYTE_3"]) << 24)
)
code = (
int(msg[f"{prefix}META_13_15"]),
int(msg[f"{prefix}META_BYTE_2"]),
int(msg[f"{prefix}META_BYTE_3"]),
)
d_rel = distance_raw * 0.05
default_distance = CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MIN <= distance_raw <= CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MAX
base_valid = (
raw not in CORNER_OBJECT_430_EMPTY_RAW_VALUES and
distance_raw not in (0, 8000, 8191) and
not default_distance and
0.2 < d_rel < CORNER_OBJECT_430_MAX_DREL
)
candidate_valid = (
base_valid and
slot_index + 1 not in CORNER_OBJECT_430_CANDIDATE_EXCLUDED_SLOTS and
code[2] in CORNER_OBJECT_430_CANDIDATE_META_BYTE_3 and
code[1] in CORNER_OBJECT_430_STRONG_META_BYTE_2 + CORNER_OBJECT_430_WEAK_META_BYTE_2
)
bins.append({
"msg_index": msg_index,
"slot_index": slot_index,
"distance_raw": distance_raw,
"d_rel": d_rel,
"code": code,
"candidate_valid": candidate_valid,
})
supported_bins = []
candidates = [b for b in bins if b["candidate_valid"]]
for b in candidates:
support = 1
for other in candidates:
if other is b:
continue
if abs(other["msg_index"] - b["msg_index"]) > 1:
continue
if abs(other["slot_index"] - b["slot_index"]) > 2:
continue
if abs(other["distance_raw"] - b["distance_raw"]) > CORNER_OBJECT_430_CANDIDATE_RAW_DELTA:
continue
support += 1
min_support = (CORNER_OBJECT_430_STRONG_MIN_SUPPORT if b["code"][1] in CORNER_OBJECT_430_STRONG_META_BYTE_2
else CORNER_OBJECT_430_WEAK_MIN_SUPPORT)
if support >= min_support:
supported_bins.append({**b, "support": support})
clusters = []
for b in sorted(supported_bins, key=lambda item: item["distance_raw"]):
if not clusters or b["distance_raw"] - clusters[-1][-1]["distance_raw"] > CORNER_OBJECT_430_CLUSTER_RAW_GAP:
clusters.append([b])
else:
clusters[-1].append(b)
clusters = sorted(clusters, key=lambda cluster: sum(b["distance_raw"] for b in cluster) / len(cluster))[:CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE]
cluster_objects = []
for cluster in clusters:
msg_index = sum(b["msg_index"] for b in cluster) / len(cluster)
slot = sum(b["slot_index"] + 1 for b in cluster) / len(cluster)
lateral_cell = (CORNER_OBJECT_430_LATERAL_CELL_MSG_WEIGHT * msg_index +
CORNER_OBJECT_430_LATERAL_CELL_SLOT_WEIGHT * slot)
mapped_cell = lateral_cell if side_sign > 0.0 else CORNER_OBJECT_430_RIGHT_CELL_MIRROR - lateral_cell
y_abs = max(CORNER_OBJECT_430_MIN_ABS_YREL,
min(CORNER_OBJECT_430_MAX_ABS_YREL,
CORNER_OBJECT_430_YREL_OFFSET - CORNER_OBJECT_430_YREL_SCALE * mapped_cell))
cluster_objects.append({
"d_rel": sum(b["d_rel"] for b in cluster) / len(cluster),
"y_rel": side_sign * y_abs,
"code": max((b["code"] for b in cluster), key=lambda code: sum(1 for item in cluster if item["code"] == code)),
})
active_t_ids = set()
side_track_ids = [
CORNER_OBJECT_430_TRACK_ID_OFFSET + track_base + slot
for slot in range(CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE)
]
unmatched_track_ids = {t_id for t_id in side_track_ids if t_id in self.corner_object_430_prev_d_rel}
unused_track_ids = [t_id for t_id in side_track_ids if t_id not in unmatched_track_ids]
for cluster in cluster_objects:
d_rel = cluster["d_rel"]
code = cluster["code"]
matched_t_id = None
if unmatched_track_ids:
nearest_t_id = min(unmatched_track_ids, key=lambda t_id: abs(d_rel - self.corner_object_430_prev_d_rel[t_id]))
if abs(d_rel - self.corner_object_430_prev_d_rel[nearest_t_id]) <= CORNER_OBJECT_430_TRACK_MATCH_MAX_DREL_DELTA:
matched_t_id = nearest_t_id
unmatched_track_ids.remove(matched_t_id)
if matched_t_id is None and unused_track_ids:
matched_t_id = unused_track_ids.pop(0)
if matched_t_id is None:
continue
t_id = matched_t_id
active_t_ids.add(t_id)
prev_d_rel = self.corner_object_430_prev_d_rel.get(t_id)
prev_code = self.corner_object_430_prev_code.get(t_id)
self.corner_object_430_prev_d_rel[t_id] = d_rel
self.corner_object_430_prev_y_rel[t_id] = cluster["y_rel"]
self.corner_object_430_prev_code[t_id] = code
reset_track = prev_d_rel is None or code != prev_code or abs(d_rel - prev_d_rel) > CORNER_OBJECT_430_MAX_DREL_DELTA
if reset_track:
self.corner_object_430_prev_v_rel.pop(t_id, None)
self.corner_object_430_prev_yv_rel.pop(t_id, None)
self.corner_object_430_history.pop(t_id, None)
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
history = self.corner_object_430_history.setdefault(t_id, deque(maxlen=CORNER_OBJECT_430_HISTORY_SIZE))
history.append((d_rel, cluster["y_rel"]))
if len(history) < CORNER_OBJECT_430_MIN_HISTORY:
self._clear_point(t_id)
continue
window_dt = CORNER_OBJECT_430_DT * (len(history) - 1)
first_d_rel, first_y_rel = history[0]
hist_v_rel = (d_rel - first_d_rel) / window_dt
if abs(hist_v_rel) > CORNER_OBJECT_430_MAX_ABS_VREL:
self.corner_object_430_prev_v_rel.pop(t_id, None)
self.corner_object_430_prev_yv_rel.pop(t_id, None)
self.corner_object_430_history.pop(t_id, None)
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
self._clear_point(t_id)
continue
prev_v_rel = self.corner_object_430_prev_v_rel.get(t_id, hist_v_rel)
v_rel = (1.0 - CORNER_OBJECT_430_VREL_ALPHA) * prev_v_rel + CORNER_OBJECT_430_VREL_ALPHA * hist_v_rel
self.corner_object_430_prev_v_rel[t_id] = v_rel
inward_steps = 0
usable_steps = 0
prev_abs_y = abs(history[0][1])
for _, y_rel in list(history)[1:]:
abs_y = abs(y_rel)
delta = prev_abs_y - abs_y
if abs(delta) > 1e-3:
usable_steps += 1
if delta > 0.0:
inward_steps += 1
prev_abs_y = abs_y
net_inward_y = abs(first_y_rel) - abs(cluster["y_rel"])
inward_ratio = inward_steps / usable_steps if usable_steps > 0 else 0.0
hist_yv_rel = (cluster["y_rel"] - first_y_rel) / window_dt
recent_inward_y = abs(history[-3][1]) - abs(cluster["y_rel"]) if len(history) >= 3 else net_inward_y
if (net_inward_y < CORNER_OBJECT_430_MIN_INWARD_YREL_DELTA or
recent_inward_y < CORNER_OBJECT_430_MIN_RECENT_INWARD_YREL_DELTA or
inward_ratio < CORNER_OBJECT_430_MIN_INWARD_RATIO or
abs(hist_yv_rel) > CORNER_OBJECT_430_MAX_ABS_YVREL):
hist_yv_rel = 0.0
inward_motion_candidate = hist_yv_rel != 0.0 and abs(cluster["y_rel"]) <= CORNER_OBJECT_430_INWARD_KEEP_YVREL_ABS_YREL
inward_center_candidate = inward_motion_candidate and abs(cluster["y_rel"]) <= CORNER_OBJECT_430_INWARD_CENTER_ABS_YREL
y_rel = cluster["y_rel"]
if inward_motion_candidate:
if inward_center_candidate:
self.corner_object_430_noncenter_inward_frames[t_id] = 0
prev_yv_rel = self.corner_object_430_prev_yv_rel.get(t_id, hist_yv_rel)
yv_rel = (1.0 - CORNER_OBJECT_430_YVREL_ALPHA) * prev_yv_rel + CORNER_OBJECT_430_YVREL_ALPHA * hist_yv_rel
else:
noncenter_frames = self.corner_object_430_noncenter_inward_frames.get(t_id, 0) + 1
self.corner_object_430_noncenter_inward_frames[t_id] = noncenter_frames
if noncenter_frames <= CORNER_OBJECT_430_EARLY_INWARD_NONCENTER_FRAMES:
prev_yv_rel = self.corner_object_430_prev_yv_rel.get(t_id, hist_yv_rel)
yv_rel = (1.0 - CORNER_OBJECT_430_YVREL_ALPHA) * prev_yv_rel + CORNER_OBJECT_430_YVREL_ALPHA * hist_yv_rel
else:
yv_rel = 0.0
if not inward_center_candidate and abs(y_rel) < CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL:
y_rel = math.copysign(CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL, y_rel)
else:
hist_yv_rel = 0.0
yv_rel = 0.0
self.corner_object_430_noncenter_inward_frames[t_id] = 0
if abs(y_rel) < CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL:
y_rel = math.copysign(CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL, y_rel)
self.corner_object_430_prev_yv_rel[t_id] = yv_rel
self.pts[t_id].measured = True
self.pts[t_id].trackId = t_id
self.pts[t_id].dRel = d_rel
self.pts[t_id].yRel = y_rel
self.pts[t_id].vRel = v_rel
self.pts[t_id].vLead = v_rel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = yv_rel
side_track_count = CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * CORNER_OBJECT_430_SLOTS_PER_MSG
for slot in range(side_track_count):
t_id = CORNER_OBJECT_430_TRACK_ID_OFFSET + track_base + slot
if t_id in active_t_ids:
continue
self.corner_object_430_prev_d_rel.pop(t_id, None)
self.corner_object_430_prev_v_rel.pop(t_id, None)
self.corner_object_430_prev_y_rel.pop(t_id, None)
self.corner_object_430_prev_yv_rel.pop(t_id, None)
self.corner_object_430_prev_code.pop(t_id, None)
self.corner_object_430_history.pop(t_id, None)
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
self._clear_point(t_id)
def _clear_point(self, t_id):
self.pts[t_id].measured = False
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
def _clear_corner_objects(self):
for slot in range(CORNER_OBJECT_235_MSG_COUNT):
self._clear_point(CORNER_OBJECT_235_TRACK_ID_OFFSET + slot)
self.corner_object_track_ids.clear_source("corner235")
def _clear_corner_objects_180(self):
for slot in range(CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG):
self._clear_point(CORNER_OBJECT_180_TRACK_ID_OFFSET + slot)
self.corner_object_track_ids.clear_source("corner180")
def _clear_corner_objects_430(self):
self.corner_object_430_prev_d_rel.clear()
self.corner_object_430_prev_v_rel.clear()
self.corner_object_430_prev_y_rel.clear()
self.corner_object_430_prev_yv_rel.clear()
self.corner_object_430_prev_code.clear()
self.corner_object_430_history.clear()
self.corner_object_430_noncenter_inward_frames.clear()
for slot in range(CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * 2 * CORNER_OBJECT_430_SLOTS_PER_MSG):
self._clear_point(CORNER_OBJECT_430_TRACK_ID_OFFSET + slot)
def _update_scc(self, updated_messages):
cpt = self.rcp_scc.vl
t_id = SCC_TID
if self.canfd:
dRel = cpt["SCC_CONTROL"]['ACC_ObjDist']
vRel = cpt["SCC_CONTROL"]['ACC_ObjRelSpd']
new_pts = abs(dRel - self.dRel_last) > 3 or abs(vRel - self.vRel_last) > 1
vLead = vRel + self.v_ego
valid = 0 < dRel < 150 and not new_pts #cpt["SCC_CONTROL"]['OBJ_STATUS'] and dRel < 150
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = dRel
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = vRel
self.pts[t_id].vLead = vLead
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0 #float('nan')
else:
dRel = cpt["SCC11"]['ACC_ObjDist']
vRel = cpt["SCC11"]['ACC_ObjRelSpd']
new_pts = abs(dRel - self.dRel_last) > 3 or abs(vRel - self.vRel_last) > 1
vLead = vRel + self.v_ego
valid = cpt["SCC11"]['ACC_ObjStatus'] and dRel < 150 and not new_pts
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = dRel
self.pts[t_id].yRel = -cpt["SCC11"]['ACC_ObjLatPos'] # in car frame's y axis, left is negative
self.pts[t_id].vRel = vRel
self.pts[t_id].vLead = vLead
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0 #float('nan')
self.dRel_last = dRel
self.vRel_last = vRel

0
artifacts/package_runtime/iqdbc/car/hyundai/tests/__init__.py Normal file
View File

Some files were not shown because too many files have changed in this diff Show More