Reapply "update to new opendbc API (#32009)" (#33151)

This reverts commit ac13000.

Author: sshane

opendbc

@@ -18,6 +18,7 @@
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 from openpilot.selfdrive.controls.lib.events import Events
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
+from openpilot.selfdrive.pandad import can_capnp_to_list
 
 ButtonType = car.CarState.ButtonEvent.Type
 GearShifter = car.CarState.GearShifter
@@ -232,9 +233,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

@@ -15,6 +15,7 @@
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+from openpilot.selfdrive.pandad import can_capnp_to_list
 from openpilot.selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator
 
 ALL_ECUS = {ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()}
@@ -128,7 +129,7 @@ def test_car_interfaces(self, car_name, data):
 
     # Test radar fault
     if not car_params.radarUnavailable and radar_interface.rcp is not None:
-      cans = [messaging.new_message('can', 1).to_bytes() for _ in range(5)]
+      cans = can_capnp_to_list([messaging.new_message('can', 1).to_bytes() for _ in range(5)])
       rr = radar_interface.update(cans)
       assert rr is None or len(rr.errors) > 0
 

selfdrive/car/tests/test_car_interfaces.py

@@ -19,6 +19,7 @@
 from openpilot.selfdrive.car.tests.routes import non_tested_cars, routes, CarTestRoute
 from openpilot.selfdrive.car.values import Platform
 from openpilot.selfdrive.car.card import Car
+from openpilot.selfdrive.pandad import can_capnp_to_list
 from openpilot.selfdrive.test.helpers import read_segment_list
 from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
 from openpilot.tools.lib.logreader import LogReader, internal_source, openpilotci_source
@@ -237,7 +238,7 @@ def test_radar_interface(self):
     # start parsing CAN messages after we've left ELM mode and can expect CAN traffic
     error_cnt = 0
     for i, msg in enumerate(self.can_msgs[self.elm_frame:]):
-      rr = RI.update((msg.as_builder().to_bytes(),))
+      rr = RI.update(can_capnp_to_list((msg.as_builder().to_bytes(),)))
       if rr is not None and i > 50:
         error_cnt += car.RadarData.Error.canError in rr.errors
     self.assertEqual(error_cnt, 0)

selfdrive/car/tests/test_models.py

@@ -1,4 +1,4 @@
-Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'opendbc_python', 'np_version')
+Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'opendbc_python', 'pandad_python', 'np_version')
 
 gen = "c_generated_code"
 
@@ -66,7 +66,7 @@ lenv.Clean(generated_files, Dir(gen))
 generated_long = lenv.Command(generated_files,
                               source_list,
                               f"cd {Dir('.').abspath} && python3 long_mpc.py")
-lenv.Depends(generated_long, [msgq_python, common_python, opendbc_python])
+lenv.Depends(generated_long, [msgq_python, common_python, opendbc_python, pandad_python])
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
 lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -10,8 +10,8 @@
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_CTRL, Ratekeeper, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
-
 from openpilot.common.simple_kalman import KF1D
+from openpilot.selfdrive.pandad import can_capnp_to_list
 
 
 # Default lead acceleration decay set to 50% at 1s
@@ -307,7 +307,7 @@ def main():
 
   while 1:
     can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
-    rr = RI.update(can_strings)
+    rr = RI.update(can_capnp_to_list(can_strings))
     sm.update(0)
     if rr is None:
       continue

selfdrive/controls/radard.py

@@ -6,6 +6,7 @@
 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 import can_capnp_to_list
 from openpilot.tools.plotjuggler.juggle import DEMO_ROUTE
 
 N_RUNS = 10
@@ -25,12 +26,13 @@ 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 = [m.as_builder().to_bytes() for m in tm.can_msgs]
     start_t = time.process_time_ns()
     for msg in msgs:
+      can_list = can_capnp_to_list([msg])
       for cp in tm.CI.can_parsers:
         if cp is not None:
-          cp.update_strings(msg)
+          cp.update_strings(can_list)
     ets.append((time.process_time_ns() - start_t) * 1e-6)
 
   print(f'{len(tm.can_msgs)} CAN packets, {N_RUNS} runs')

selfdrive/debug/check_can_parser_performance.py

@@ -6,6 +6,8 @@ panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc'])
 env.Program('pandad', ['main.cc', 'pandad.cc'], LIBS=[panda] + libs)
 env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
 
-envCython.Program('pandad_api_impl.so', 'pandad_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
+pandad_python = envCython.Program('pandad_api_impl.so', 'pandad_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
+Export('pandad_python')
+
 if GetOption('extras'):
   env.Program('tests/test_pandad_usbprotocol', ['tests/test_pandad_usbprotocol.cc'], LIBS=[panda] + libs)

selfdrive/pandad/SConscript

@@ -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,11 +1,12 @@
 #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) {
+void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendcan, bool valid) {
   MessageBuilder msg;
   auto event = msg.initEvent(valid);
 
-  auto canData = sendCan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size());
+  auto canData = sendcan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size());
   int j = 0;
   for (auto it = can_list.begin(); it != can_list.end(); it++, j++) {
     auto c = canData[j];
@@ -18,3 +19,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);
 }
+
+// Converts a vector of Cap'n Proto serialized can strings into a vector of CanData structures.
+void can_capnp_to_can_list_cpp(const std::vector<std::string> &strings, std::vector<CanData> &can_list, bool sendcan) {
+  AlignedBuffer aligned_buf;
+  can_list.reserve(strings.size());
+
+  for (const auto &str : strings) {
+    // extract the messages
+    capnp::FlatArrayMessageReader reader(aligned_buf.align(str.data(), str.size()));
+    cereal::Event::Reader event = reader.getRoot<cereal::Event>();
+
+    auto frames = sendcan ? event.getSendcan() : event.getCan();
+
+    // Add new CanData entry
+    CanData &can_data = can_list.emplace_back();
+    can_data.nanos = event.getLogMonoTime();
+    can_data.frames.reserve(frames.size());
+
+    // Populate CAN frames
+    for (const auto &frame : frames) {
+      CanFrame &can_frame = can_data.frames.emplace_back();
+      can_frame.src = frame.getSrc();
+      can_frame.address = frame.getAddress();
+
+      // Copy CAN data
+      auto dat = frame.getDat();
+      can_frame.dat.assign(dat.begin(), dat.end());
+    }
+  }
+}

selfdrive/pandad/can_list_to_can_capnp.cc

@@ -1,17 +1,30 @@
 # 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:
     long address
     string dat
     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_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
@@ -27,3 +40,17 @@ def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
   cdef string out
   can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan', valid)
   return out
+
+def can_capnp_to_list(strings, msgtype='can'):
+  cdef vector[CanData] data
+  can_capnp_to_can_list_cpp(strings, data, msgtype == 'sendcan')
+
+  result = []
+  cdef CanData *d
+  cdef vector[CanData].iterator it = data.begin()
+  while it != data.end():
+    d = &deref(it)
+    frames = [[f.address, (<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 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 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",