From 0e705536041e23d33041c9da4e56b2c1c8f20a31 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 14 Apr 2023 11:23:22 -0700 Subject: [PATCH] Hyundai: factor out bus numbers (#27903) * mv bus * mv that * if len * fix tests * use that * same name old-commit-hash: 08e2149b0f0e62db8a30d4355d458b530167a7c0 --- selfdrive/car/hyundai/carcontroller.py | 24 +++---- selfdrive/car/hyundai/carstate.py | 6 +- selfdrive/car/hyundai/hyundaicanfd.py | 88 +++++++++++++++++--------- selfdrive/car/hyundai/interface.py | 31 +++++---- selfdrive/car/tests/routes.py | 1 + 5 files changed, 94 insertions(+), 56 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 257203849..ac74d2cc5 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -5,6 +5,7 @@ from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.hyundai import hyundaicanfd, hyundaican +from selfdrive.car.hyundai.hyundaicanfd import CanBus from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -44,6 +45,7 @@ def process_hud_alert(enabled, fingerprint, hud_control): class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP + self.CAN = CanBus(CP) self.params = CarControllerParams(CP) self.packer = CANPacker(dbc_name) self.angle_limit_counter = 0 @@ -85,12 +87,12 @@ class CarController: # for longitudinal control, either radar or ADAS driving ECU addr, bus = 0x7d0, 0 if self.CP.flags & HyundaiFlags.CANFD_HDA2.value: - addr, bus = 0x730, 5 + addr, bus = 0x730, self.CAN.ECAN can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus]) # for blinkers if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: - can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 5]) + can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN]) # >90 degree steering fault prevention # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault @@ -112,25 +114,25 @@ class CarController: hda2_long = hda2 and self.CP.openpilotLongitudinalControl # steering control - can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer)) + can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, lat_active, apply_steer)) # disable LFA on HDA2 if self.frame % 5 == 0 and hda2: - can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, CS.cam_0x2a4)) + can_sends.append(hyundaicanfd.create_cam_0x2a4(self.packer, self.CAN, CS.cam_0x2a4)) # LFA and HDA icons if self.frame % 5 == 0 and (not hda2 or hda2_long): - can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled)) + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled)) # blinkers if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: - can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.frame, CC.leftBlinker, CC.rightBlinker)) + can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker)) if self.CP.openpilotLongitudinalControl: if hda2: - can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame)) + can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: - can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, + can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, set_speed_in_units)) self.accel_last = accel else: @@ -139,11 +141,11 @@ class CarController: # cruise cancel if CC.cruiseControl.cancel: if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: - can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, CS.cruise_info)) + can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CAN, CS.cruise_info)) self.last_button_frame = self.frame else: for _ in range(20): - can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, CS.buttons_counter+1, Buttons.CANCEL)) + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL)) self.last_button_frame = self.frame # cruise standstill resume @@ -153,7 +155,7 @@ class CarController: pass else: for _ in range(20): - can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, CS.buttons_counter+1, Buttons.RES_ACCEL)) + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL)) self.last_button_frame = self.frame else: can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active, diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 427f8cedb..cec2c3e69 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -6,7 +6,7 @@ from cereal import car from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.hyundai.hyundaicanfd import get_e_can_bus +from selfdrive.car.hyundai.hyundaicanfd import CanBus from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams from selfdrive.car.interfaces import CarStateBase @@ -516,7 +516,7 @@ class CarState(CarStateBase): ("ACCELERATOR_BRAKE_ALT", 100), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, get_e_can_bus(CP)) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).ECAN) @staticmethod def get_cam_can_parser_canfd(CP): @@ -543,4 +543,4 @@ class CarState(CarStateBase): ("SCC_CONTROL", 50), ] - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus(CP).CAM) diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index af7239571..3717a4590 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,14 +1,44 @@ +import math + from common.numpy_fast import clip from selfdrive.car.hyundai.values import HyundaiFlags -def get_e_can_bus(CP): - # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars - # have a different harness than the HDA1 and non-HDA variants in order to split - # a different bus, since the steering is done by different ECUs. - return 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4 + +class CanBus: + def __init__(self, CP, hda2=None, fingerprint=None): + if CP is None: + assert None not in (hda2, fingerprint) + num = math.ceil(max([k for k, v in fingerprint.items() if len(v)], default=1) / 4) + else: + hda2 = CP.flags & HyundaiFlags.CANFD_HDA2.value + num = len(CP.safetyConfigs) + + # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars + # have a different harness than the HDA1 and non-HDA variants in order to split + # a different bus, since the steering is done by different ECUs. + self._a, self._e = 1, 0 + if hda2: + self._a, self._e = 0, 1 + + offset = 4*(num - 1) + self._a += offset + self._e += offset + self._cam = 2 + offset + + @property + def ECAN(self): + return self._e + + @property + def ACAN(self): + return self._a + + @property + def CAM(self): + return self._cam -def create_steering_messages(packer, CP, enabled, lat_active, apply_steer): +def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer): ret = [] @@ -26,45 +56,45 @@ def create_steering_messages(packer, CP, enabled, lat_active, apply_steer): if CP.flags & HyundaiFlags.CANFD_HDA2: if CP.openpilotLongitudinalControl: - ret.append(packer.make_can_msg("LFA", 5, values)) - ret.append(packer.make_can_msg("LKAS", 4, values)) + ret.append(packer.make_can_msg("LFA", CAN.ECAN, values)) + ret.append(packer.make_can_msg("LKAS", CAN.ACAN, values)) else: - ret.append(packer.make_can_msg("LFA", 4, values)) + ret.append(packer.make_can_msg("LFA", CAN.ECAN, values)) return ret -def create_cam_0x2a4(packer, camera_values): +def create_cam_0x2a4(packer, CAN, camera_values): camera_values.update({ "BYTE7": 0, }) - return packer.make_can_msg("CAM_0x2a4", 4, camera_values) + return packer.make_can_msg("CAM_0x2a4", CAN.ACAN, camera_values) -def create_buttons(packer, CP, cnt, btn): +def create_buttons(packer, CP, CAN, cnt, btn): values = { "COUNTER": cnt, "SET_ME_1": 1, "CRUISE_BUTTONS": btn, } - bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 6 + bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM return packer.make_can_msg("CRUISE_BUTTONS", bus, values) -def create_acc_cancel(packer, CP, cruise_info_copy): +def create_acc_cancel(packer, CAN, cruise_info_copy): values = cruise_info_copy values.update({ "ACCMode": 4, }) - return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_lfahda_cluster(packer, CP, enabled): +def create_lfahda_cluster(packer, CAN, enabled): values = { "HDA_ICON": 1 if enabled else 0, "LFA_ICON": 2 if enabled else 0, } - return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values) + return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_override, set_speed): +def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -92,15 +122,15 @@ def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_ove "DISTANCE_SETTING": 4, } - return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) + return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_spas_messages(packer, frame, left_blink, right_blink): +def create_spas_messages(packer, CAN, frame, left_blink, right_blink): ret = [] values = { } - ret.append(packer.make_can_msg("SPAS1", 5, values)) + ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values)) blink = 0 if left_blink: @@ -110,12 +140,12 @@ def create_spas_messages(packer, frame, left_blink, right_blink): values = { "BLINKER_CONTROL": blink, } - ret.append(packer.make_can_msg("SPAS2", 5, values)) + ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values)) return ret -def create_adrv_messages(packer, frame): +def create_adrv_messages(packer, CAN, frame): # messages needed to car happy after disabling # the ADAS Driving ECU to do longitudinal control @@ -123,7 +153,7 @@ def create_adrv_messages(packer, frame): values = { } - ret.append(packer.make_can_msg("ADRV_0x51", 4, values)) + ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values)) if frame % 2 == 0: values = { @@ -133,7 +163,7 @@ def create_adrv_messages(packer, frame): 'SET_ME_FC': 0xfc, 'SET_ME_9': 0x9, } - ret.append(packer.make_can_msg("ADRV_0x160", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values)) if frame % 5 == 0: values = { @@ -142,25 +172,25 @@ def create_adrv_messages(packer, frame): 'SET_ME_TMP_F': 0xf, 'SET_ME_TMP_F_2': 0xf, } - ret.append(packer.make_can_msg("ADRV_0x1ea", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values)) values = { 'SET_ME_E1': 0xe1, 'SET_ME_3A': 0x3a, } - ret.append(packer.make_can_msg("ADRV_0x200", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values)) if frame % 20 == 0: values = { 'SET_ME_15': 0x15, } - ret.append(packer.make_can_msg("ADRV_0x345", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values)) if frame % 100 == 0: values = { 'SET_ME_22': 0x22, 'SET_ME_41': 0x41, } - ret.append(packer.make_can_msg("ADRV_0x1da", 5, values)) + ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values)) return ret diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index bdb75c1fb..93f3a75be 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,7 +2,7 @@ from cereal import car from panda import Panda from common.conversions import Conversions as CV -from selfdrive.car.hyundai.hyundaicanfd import get_e_can_bus +from selfdrive.car.hyundai.hyundaicanfd import CanBus from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config @@ -28,17 +28,20 @@ class CarInterface(CarInterfaceBase): # added to selfdrive/car/tests/routes.py, we can remove it from this list. ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, } + hda2 = Ecu.adas in [fw.ecu for fw in car_fw] + CAN = CanBus(None, hda2, fingerprint) + if candidate in CANFD_CAR: # detect HDA2 with ADAS Driving ECU - if Ecu.adas in [fw.ecu for fw in car_fw]: + if hda2: ret.flags |= HyundaiFlags.CANFD_HDA2.value else: # non-HDA2 - if 0x1cf not in fingerprint[4]: + if 0x1cf not in fingerprint[CAN.ECAN]: ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value # ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead - if 0x130 not in fingerprint[4]: - if 0x40 not in fingerprint[4]: + if 0x130 not in fingerprint[CAN.ECAN]: + if 0x40 not in fingerprint[CAN.ECAN]: ret.flags |= HyundaiFlags.CANFD_ALT_GEARS_2.value else: ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value @@ -248,21 +251,23 @@ class CarInterface(CarInterfaceBase): # *** feature detection *** if candidate in CANFD_CAR: - ret.enableBsm = 0x1e5 in fingerprint[get_e_can_bus(ret)] + ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN] else: ret.enableBsm = 0x58b in fingerprint[0] # *** panda safety config *** if candidate in CANFD_CAR: - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput), - get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd)] + cfgs = [get_safety_config(car.CarParams.SafetyModel.hyundaiCanfd), ] + if CAN.ECAN >= 4: + cfgs.insert(0, get_safety_config(car.CarParams.SafetyModel.noOutput)) + ret.safetyConfigs = cfgs if ret.flags & HyundaiFlags.CANFD_HDA2: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2 if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC: - ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC else: if candidate in LEGACY_SAFETY_MODE_CAR: # these cars require a special panda safety mode due to missing counters and checksums in the messages @@ -297,12 +302,12 @@ class CarInterface(CarInterfaceBase): if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value): addr, bus = 0x7d0, 0 if CP.flags & HyundaiFlags.CANFD_HDA2.value: - addr, bus = 0x730, 5 + addr, bus = 0x730, CanBus(CP).ECAN disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') # for blinkers if CP.flags & HyundaiFlags.ENABLE_BLINKERS: - disable_ecu(logcan, sendcan, bus=5, addr=0x7B1, com_cont_req=b'\x28\x83\x01') + disable_ecu(logcan, sendcan, bus=CanBus(CP.ECAN), addr=0x7B1, com_cont_req=b'\x28\x83\x01') def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 90a8ad566..3935461e6 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -128,6 +128,7 @@ routes = [ CarTestRoute("d624b3d19adce635|2020-08-01--14-59-12", HYUNDAI.VELOSTER), CarTestRoute("d545129f3ca90f28|2022-10-19--09-22-54", HYUNDAI.KIA_EV6), # HDA2 CarTestRoute("68d6a96e703c00c9|2022-09-10--16-09-39", HYUNDAI.KIA_EV6), # HDA1 + CarTestRoute("9b25e8c1484a1b67|2023-04-13--10-41-45", HYUNDAI.KIA_EV6), CarTestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021), CarTestRoute("c58dfc9fc16590e0|2023-01-14--13-51-48", HYUNDAI.KIA_K5_HEV_2020), CarTestRoute("50c6c9b85fd1ff03|2020-10-26--17-56-06", HYUNDAI.KIA_NIRO_EV),