use new opendbc api

Author: deanlee

opendbc

@@ -3,6 +3,7 @@
 from cereal import car
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 from openpilot.selfdrive.car.chrysler.values import DBC
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 
 RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2))  # c_ messages 706,...,724
 RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2))  # d_ messages
@@ -48,7 +49,7 @@ def update(self, can_strings):
     if self.rcp is None or self.CP.radarUnavailable:
       return super().update(None)
 
-    vls = self.rcp.update_strings(can_strings)
+    vls = self.rcp.update_strings(can_capnp_to_list(can_strings))
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/chrysler/radar_interface.py

@@ -5,6 +5,7 @@
 from openpilot.selfdrive.car.ford.fordcan import CanBus
 from openpilot.selfdrive.car.ford.values import DBC, RADAR
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 
 DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
 
@@ -52,7 +53,7 @@ def update(self, can_strings):
     if self.rcp is None:
       return super().update(None)
 
-    vls = self.rcp.update_strings(can_strings)
+    vls = self.rcp.update_strings(can_capnp_to_list(can_strings))
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/ford/radar_interface.py

@@ -5,6 +5,7 @@
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.gm.values import DBC, CanBus
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 
 RADAR_HEADER_MSG = 1120
 SLOT_1_MSG = RADAR_HEADER_MSG + 1
@@ -46,7 +47,7 @@ def update(self, can_strings):
     if self.rcp is None:
       return super().update(None)
 
-    vls = self.rcp.update_strings(can_strings)
+    vls = self.rcp.update_strings(can_capnp_to_list(can_strings))
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/gm/radar_interface.py

@@ -3,6 +3,7 @@
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 from openpilot.selfdrive.car.honda.values import DBC
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 
 
 def _create_nidec_can_parser(car_fingerprint):
@@ -36,7 +37,7 @@ def update(self, can_strings):
     if self.radar_off_can:
       return super().update(None)
 
-    vls = self.rcp.update_strings(can_strings)
+    vls = self.rcp.update_strings(can_capnp_to_list(can_strings))
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/honda/radar_interface.py

@@ -4,6 +4,7 @@
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 from openpilot.selfdrive.car.hyundai.values import DBC
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 
 RADAR_START_ADDR = 0x500
 RADAR_MSG_COUNT = 32
@@ -32,7 +33,7 @@ def update(self, can_strings):
     if self.radar_off_can or (self.rcp is None):
       return super().update(None)
 
-    vls = self.rcp.update_strings(can_strings)
+    vls = self.rcp.update_strings(can_capnp_to_list(can_strings))
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/hyundai/radar_interface.py

@@ -19,6 +19,7 @@
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
 from openpilot.selfdrive.controls.lib.events import Events
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 
 ButtonType = car.CarState.ButtonEvent.Type
 GearShifter = car.CarState.GearShifter
@@ -235,9 +236,10 @@ def _update(self, c: car.CarControl) -> car.CarState:
 
   def update(self, c: car.CarControl, can_strings: list[bytes]) -> car.CarState:
     # parse can
+    can_list = can_capnp_to_list(can_strings)
     for cp in self.can_parsers:
       if cp is not None:
-        cp.update_strings(can_strings)
+        cp.update_strings(can_list)
 
     # get CarState
     ret = self._update(c)

selfdrive/car/interfaces.py

@@ -3,6 +3,7 @@
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 
 
 class RadarInterface(RadarInterfaceBase):
@@ -33,7 +34,7 @@ def update(self, can_strings):
     if self.rcp is None:
       return super().update(None)
 
-    values = self.rcp.update_strings(can_strings)
+    values = self.rcp.update_strings(can_capnp_to_list(can_strings))
     self.updated_messages.update(values)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/tesla/radar_interface.py

@@ -3,6 +3,7 @@
 from cereal import car
 from openpilot.selfdrive.car.toyota.values import DBC, TSS2_CAR
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 
 
 def _create_radar_can_parser(car_fingerprint):
@@ -42,7 +43,7 @@ def update(self, can_strings):
     if self.rcp is None:
       return super().update(None)
 
-    vls = self.rcp.update_strings(can_strings)
+    vls = self.rcp.update_strings(can_capnp_to_list(can_strings))
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

selfdrive/car/toyota/radar_interface.py

@@ -6,8 +6,10 @@
 from cereal import car
 from openpilot.selfdrive.car.tests.routes import CarTestRoute
 from openpilot.selfdrive.car.tests.test_models import TestCarModelBase
+from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list
 from openpilot.tools.plotjuggler.juggle import DEMO_ROUTE
 
+
 N_RUNS = 10
 
 
@@ -25,7 +27,7 @@ class CarModelTestCase(TestCarModelBase):
   CC = car.CarControl.new_message()
   ets = []
   for _ in tqdm(range(N_RUNS)):
-    msgs = [(m.as_builder().to_bytes(),) for m in tm.can_msgs]
+    msgs = can_capnp_to_list([m.as_builder().to_bytes() for m in tm.can_msgs])
     start_t = time.process_time_ns()
     for msg in msgs:
       for cp in tm.CI.can_parsers:

selfdrive/debug/check_can_parser_performance.py

@@ -1,6 +1,7 @@
 # Cython, now uses scons to build
-from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp
+from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp, can_capnp_to_list
 assert can_list_to_can_capnp
+assert can_capnp_to_list
 
 def can_capnp_to_can_list(can, src_filter=None):
   ret = []

selfdrive/pandad/__init__.py

@@ -1,5 +1,6 @@
 #include "cereal/messaging/messaging.h"
 #include "selfdrive/pandad/panda.h"
+#include "opendbc/can/common.h"
 
 void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan, bool valid) {
   MessageBuilder msg;
@@ -19,3 +20,33 @@ void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::stri
   kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>((unsigned char *)out.data(), msg_size));
   capnp::writeMessage(output_stream, msg);
 }
+
+void can_capnp_to_can_list_cpp(const std::vector<std::string> &strings, std::vector<CanData> &can_data, bool sendcan) {
+  kj::Array<capnp::word> aligned_buf;
+  can_data.reserve(strings.size());
+  for (const auto &s : strings) {
+    const size_t buf_size = (s.length() / sizeof(capnp::word)) + 1;
+    if (aligned_buf.size() < buf_size) {
+      aligned_buf = kj::heapArray<capnp::word>(buf_size);
+    }
+    memcpy(aligned_buf.begin(), s.data(), s.length());
+
+    // extract the messages
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.slice(0, buf_size));
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+    auto &can = can_data.emplace_back();
+    can.nanos = event.getLogMonoTime();
+
+    auto cans = sendcan ? event.getSendcan() : event.getCan();
+    can.frames.reserve(cans.size());
+    for (const auto &c : cans) {
+      auto &frame = can.frames.emplace_back();
+      frame.src = c.getSrc();
+      frame.address = c.getAddress();
+      auto dat = c.getDat();
+      frame.dat.resize(dat.size());
+      memcpy(frame.dat.data(), dat.begin(), dat.size());
+    }
+  }
+}

selfdrive/pandad/can_list_to_can_capnp.cc

@@ -1,8 +1,10 @@
 # distutils: language = c++
 # cython: language_level=3
+from cython.operator cimport dereference as deref, preincrement as preinc
 from libcpp.vector cimport vector
 from libcpp.string cimport string
 from libcpp cimport bool
+from libc.stdint cimport uint8_t, uint32_t, uint64_t
 
 cdef extern from "panda.h":
   cdef struct can_frame:
@@ -11,21 +13,44 @@ cdef extern from "panda.h":
     long busTime
     long src
 
+cdef extern from "opendbc/can/common.h":
+  cdef struct CanFrame:
+    long src
+    uint32_t address
+    vector[uint8_t] dat
+
+  cdef struct CanData:
+    uint64_t nanos
+    vector[CanFrame] frames
+
 cdef extern from "can_list_to_can_capnp.cc":
   void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid)
+  void can_capnp_to_can_list_cpp(const vector[string] &strings, vector[CanData] &can_data, bool sendcan)
 
 def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
-  cdef can_frame *f
   cdef vector[can_frame] can_list
-
   can_list.reserve(len(can_msgs))
+
+  cdef can_frame f
   for can_msg in can_msgs:
-    f = &(can_list.emplace_back())
     f.address = can_msg[0]
     f.busTime = can_msg[1]
     f.dat = can_msg[2]
     f.src = can_msg[3]
-
+    can_list.push_back(f)
   cdef string out
   can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan', valid)
   return out
+
+def can_capnp_to_list(strings, sendcan=False):
+  cdef vector[CanData] data
+  can_capnp_to_can_list_cpp(strings, data, sendcan)
+  result = []
+  cdef vector[CanData].iterator it = data.begin()
+  cdef CanData *d
+  while it != data.end():
+    d = &deref(it)
+    frames = [[f.address, 0, (<char *>&f.dat[0])[:f.dat.size()], f.src] for f in d.frames]
+    result.append([d.nanos, frames])
+    preinc(it)
+  return result

selfdrive/pandad/pandad_api_impl.pyx

@@ -24,6 +24,7 @@
     "from opendbc.can.parser import CANParser\n",
     "\n",
     "from openpilot.selfdrive.car.subaru.values import DBC\n",
+    "from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list\n",
     "from openpilot.tools.lib.logreader import LogReader\n",
     "\n",
     "\"\"\"\n",
@@ -50,7 +51,7 @@
     "\n",
     "  for msg in lr:\n",
     "    if msg.which() == \"can\":\n",
-    "      cp.update_strings([msg.as_builder().to_bytes()])\n",
+    "      cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()]))\n",
     "      es_distance_history.append(copy.copy(cp.vl[\"ES_Distance\"]))\n",
     "      es_brake_history.append(copy.copy(cp.vl[\"ES_Brake\"]))\n",
     "      es_status_history.append(copy.copy(cp.vl[\"ES_Status\"]))\n",

tools/car_porting/examples/subaru_long_accel.ipynb

@@ -27,6 +27,7 @@
     "from opendbc.can.parser import CANParser\n",
     "\n",
     "from openpilot.selfdrive.car.subaru.values import CanBus, DBC\n",
+    "from openpilot.selfdrive.pandad.pandad_api_impl import can_capnp_to_list\n",
     "from openpilot.tools.lib.logreader import LogReader\n",
     "\n",
     "\"\"\"\n",
@@ -50,7 +51,7 @@
     "  examples = []\n",
     "\n",
     "  for msg in can_msgs:\n",
-    "    cp.update_strings([msg.as_builder().to_bytes()])\n",
+    "    cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()]))\n",
     "    steering_torque_history.append(copy.copy(cp.vl[\"Steering_Torque\"]))\n",
     "\n",
     "  steer_warning_last = False\n",