From 7c112341c358b57ee9d0209cb2b2c53383adcca9 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 31 Jul 2024 15:37:39 -0700 Subject: [PATCH] Reapply "update to new opendbc API (#32009)" (#33151) This reverts commit ac130001cc83734118524f59a83a96a2065dd56e. --- opendbc | 2 +- selfdrive/car/interfaces.py | 4 ++- selfdrive/car/tests/test_car_interfaces.py | 3 +- selfdrive/car/tests/test_models.py | 3 +- .../lib/longitudinal_mpc_lib/SConscript | 4 +-- selfdrive/controls/radard.py | 4 +-- .../debug/check_can_parser_performance.py | 6 ++-- selfdrive/pandad/SConscript | 4 ++- selfdrive/pandad/__init__.py | 3 +- selfdrive/pandad/can_list_to_can_capnp.cc | 35 +++++++++++++++++-- selfdrive/pandad/pandad_api_impl.pyx | 29 ++++++++++++++- .../examples/subaru_long_accel.ipynb | 3 +- .../examples/subaru_steer_temp_fault.ipynb | 3 +- 13 files changed, 86 insertions(+), 17 deletions(-) diff --git a/opendbc b/opendbc index 8e9d3688412405..39a5345924d241 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 8e9d3688412405154a8189c421cfdc9d5feea715 +Subproject commit 39a5345924d2414f05c40d258d359eb8412a0b03 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 6c6e544f759a55..2121f433b7ea83 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -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) diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 4ca19f019e6df0..9e3c7d157ef223 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_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 diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 1d05b7c73140a1..22a80f359a93af 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.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) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index fc1e844d504a69..22342c00785e1f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -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") diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index c3fb60c61ad1aa..a7e3c0211d3ec7 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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 diff --git a/selfdrive/debug/check_can_parser_performance.py b/selfdrive/debug/check_can_parser_performance.py index 604a1df1248e86..a5155f0126b8dc 100755 --- a/selfdrive/debug/check_can_parser_performance.py +++ b/selfdrive/debug/check_can_parser_performance.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') diff --git a/selfdrive/pandad/SConscript b/selfdrive/pandad/SConscript index 63a2c1e650d539..dcc1f9811ee0bd 100644 --- a/selfdrive/pandad/SConscript +++ b/selfdrive/pandad/SConscript @@ -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) diff --git a/selfdrive/pandad/__init__.py b/selfdrive/pandad/__init__.py index 2c80cd03c473ec..b72c8ccb57dd14 100644 --- a/selfdrive/pandad/__init__.py +++ b/selfdrive/pandad/__init__.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 = [] diff --git a/selfdrive/pandad/can_list_to_can_capnp.cc b/selfdrive/pandad/can_list_to_can_capnp.cc index d73541c17fb8c3..ad2393b986ccff 100644 --- a/selfdrive/pandad/can_list_to_can_capnp.cc +++ b/selfdrive/pandad/can_list_to_can_capnp.cc @@ -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_list, std::string &out, bool sendCan, bool valid) { +void can_list_to_can_capnp_cpp(const std::vector &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_list, std::stri kj::ArrayOutputStream output_stream(kj::ArrayPtr((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 &strings, std::vector &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(); + + 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()); + } + } +} diff --git a/selfdrive/pandad/pandad_api_impl.pyx b/selfdrive/pandad/pandad_api_impl.pyx index dd3f3d702f6d15..8a12e1c4337b18 100644 --- a/selfdrive/pandad/pandad_api_impl.pyx +++ b/selfdrive/pandad/pandad_api_impl.pyx @@ -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: @@ -10,8 +12,19 @@ cdef extern from "panda.h": 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, (&f.dat[0])[:f.dat.size()], f.src] for f in d.frames] + result.append([d.nanos, frames]) + preinc(it) + return result diff --git a/tools/car_porting/examples/subaru_long_accel.ipynb b/tools/car_porting/examples/subaru_long_accel.ipynb index 9d18a114df3ee8..35b92702a7e991 100644 --- a/tools/car_porting/examples/subaru_long_accel.ipynb +++ b/tools/car_porting/examples/subaru_long_accel.ipynb @@ -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", diff --git a/tools/car_porting/examples/subaru_steer_temp_fault.ipynb b/tools/car_porting/examples/subaru_steer_temp_fault.ipynb index 3d5055cbc29b04..8b762fecb8461b 100644 --- a/tools/car_porting/examples/subaru_steer_temp_fault.ipynb +++ b/tools/car_porting/examples/subaru_steer_temp_fault.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",