Release 260111

opendbc_repo/opendbc/can/packer.py

@@ -0,0 +1,67 @@
+import math
+
+from opendbc.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:
+      return bytearray()
+    dat = bytearray(msg.size)
+    counter_set = False
+    for name, value in values.items():
+      sig = msg.sigs.get(name)
+      if sig is None:
+        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]):
+    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:
+        return 0, b'', bus
+      addr = msg.address
+    dat = self.pack(addr, 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