diff --git a/opendbc b/opendbc index 776bca184bc997..c3367d0d00a4f3 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 776bca184bc997b587afb20df71bc56a4890c4d8 +Subproject commit c3367d0d00a4f3c8461e3d37638a6f16b3f39aa6 diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index d9829584228b4d..cd549b8e4308a4 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -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: diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 209bbebae34d5f..f9cfdc083491d2 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/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: diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index b893babd319396..a7dda282017fbc 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/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: diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 8090f8e03e7735..0cb15c217a6fa6 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/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: diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 52600509860f5a..1fc1157689e260 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/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: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a3572238bab5af..5cebb8053f3083 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.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) diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index ae5077824bc6f2..d0e232325882cb 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.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: diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index fae6eecaf634e2..1110bbf2b5f8cc 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/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: diff --git a/selfdrive/debug/check_can_parser_performance.py b/selfdrive/debug/check_can_parser_performance.py index 604a1df1248e86..9ac17639ad2580 100755 --- a/selfdrive/debug/check_can_parser_performance.py +++ b/selfdrive/debug/check_can_parser_performance.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: diff --git a/selfdrive/pandad/__init__.py b/selfdrive/pandad/__init__.py index 8081a62dd0f1d7..c2df28e00fc296 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 9fc2648da27a62..76688dcefab3fd 100644 --- a/selfdrive/pandad/can_list_to_can_capnp.cc +++ b/selfdrive/pandad/can_list_to_can_capnp.cc @@ -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_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_list, std::stri kj::ArrayOutputStream output_stream(kj::ArrayPtr((unsigned char *)out.data(), msg_size)); capnp::writeMessage(output_stream, msg); } + +void can_capnp_to_can_list_cpp(const std::vector &strings, std::vector &can_data, bool sendcan) { + kj::Array 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(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(); + + 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()); + } + } +} diff --git a/selfdrive/pandad/pandad_api_impl.pyx b/selfdrive/pandad/pandad_api_impl.pyx index cbac8cc5a3adf0..7bc3b6c2d9b612 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: @@ -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, (&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..1cf0710d62e633 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.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", diff --git a/tools/car_porting/examples/subaru_steer_temp_fault.ipynb b/tools/car_porting/examples/subaru_steer_temp_fault.ipynb index 3d5055cbc29b04..a50ce24e06aa59 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.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",