From c36235f6fd667e84da5d7c4cb1a6475b38c0cb48 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 22 Apr 2026 17:44:22 -0500 Subject: [PATCH] add kia sportage hybrid support --- opendbc_repo/opendbc/car/fingerprints.py | 2 + .../opendbc/car/hyundai/carcontroller.py | 65 ++++-- .../opendbc/car/hyundai/fingerprints.py | 9 + .../opendbc/car/hyundai/hyundaicanfd.py | 59 +++++- opendbc_repo/opendbc/car/hyundai/interface.py | 6 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 20 +- opendbc_repo/opendbc/car/hyundai/values.py | 27 ++- opendbc_repo/opendbc/car/tests/routes.py | 1 + .../opendbc/car/torque_data/override.toml | 1 + .../opendbc/safety/modes/hyundai_canfd.h | 39 +++- .../safety/tests/test_hyundai_canfd.py | 196 +++++++++++++++++- 11 files changed, 389 insertions(+), 36 deletions(-) diff --git a/opendbc_repo/opendbc/car/fingerprints.py b/opendbc_repo/opendbc/car/fingerprints.py index 0335d2224..66033a0c2 100644 --- a/opendbc_repo/opendbc/car/fingerprints.py +++ b/opendbc_repo/opendbc/car/fingerprints.py @@ -118,6 +118,7 @@ MIGRATION = { "LEXUS RX HYBRID 2017": TOYOTA.LEXUS_RX, "HYUNDAI TUCSON HYBRID 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN, "KIA SPORTAGE HYBRID 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN, + "KIA SPORTAGE HYBRID 2026": HYUNDAI.KIA_SPORTAGE_HEV_2026, "KIA SORENTO PLUG-IN HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, "CADILLAC ESCALADE ESV PLATINUM 2019": GM.CADILLAC_ESCALADE_ESV_2019, @@ -227,6 +228,7 @@ MIGRATION = { "KIA OPTIMA HYBRID 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_H_G4_FL, "KIA SELTOS 2021": HYUNDAI.KIA_SELTOS, "KIA SPORTAGE 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN, + "KIA SPORTAGE HYBRID 2026": HYUNDAI.KIA_SPORTAGE_HEV_2026, "KIA SORENTO GT LINE 2018": HYUNDAI.KIA_SORENTO, "KIA SORENTO 4TH GEN": HYUNDAI.KIA_SORENTO_4TH_GEN, "KIA SORENTO HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 44b19fa37..3d1ed4136 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -1,12 +1,13 @@ import numpy as np from opendbc.can import CANPacker from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs -from opendbc.car.lateral import apply_driver_steer_torque_limits, common_fault_avoidance +from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance from opendbc.car.common.conversions import Conversions as CV from opendbc.car.hyundai import hyundaicanfd, hyundaican from opendbc.car.hyundai.hyundaicanfd import CanBus from opendbc.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CAR from opendbc.car.interfaces import CarControllerBase +from opendbc.car.vehicle_model import VehicleModel VisualAlert = structs.CarControl.HUDControl.VisualAlert LongCtrlState = structs.CarControl.Actuators.LongControlState @@ -49,9 +50,11 @@ class CarController(CarControllerBase): self.params = CarControllerParams(CP) self.packer = CANPacker(dbc_names[Bus.pt]) self.angle_limit_counter = 0 + self.VM = VehicleModel(CP) self.accel_last = 0 self.apply_torque_last = 0 + self.apply_angle_last = 0.0 self.car_fingerprint = CP.carFingerprint self.last_button_frame = 0 @@ -59,21 +62,41 @@ class CarController(CarControllerBase): actuators = CC.actuators hud_control = CC.hudControl - # steering torque - new_torque = int(round(actuators.torque * self.params.STEER_MAX)) - apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.params) + apply_angle = CS.out.steeringAngleDeg - # >90 degree steering fault prevention - self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive, - self.angle_limit_counter, MAX_ANGLE_FRAMES, - MAX_ANGLE_CONSECUTIVE_FRAMES) + if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: + desired_angle = float(np.clip(actuators.steeringAngleDeg, + -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, + self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) + apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, CS.out.vEgoRaw, + CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM) - if not CC.latActive: - apply_torque = 0 + if CS.out.steeringPressed and abs(CS.out.steeringTorque) > self.params.STEER_THRESHOLD: + apply_torque = self.params.ANGLE_MIN_TORQUE_REDUCTION_GAIN + elif CC.latActive and CS.out.vEgoRaw < 0.3: + apply_torque = self.params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN + else: + apply_torque = self.params.ANGLE_MAX_TORQUE_REDUCTION_GAIN if CC.latActive else 0.0 - # Hold torque with induced temporary fault when cutting the actuation bit - # FIXME: we don't use this with CAN FD? - torque_fault = CC.latActive and not apply_steer_req + apply_steer_req = CC.latActive and apply_torque > 0.0 + torque_fault = False + self.apply_angle_last = apply_angle + else: + # steering torque + new_torque = int(round(actuators.torque * self.params.STEER_MAX)) + apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.params) + + # >90 degree steering fault prevention + self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive, + self.angle_limit_counter, MAX_ANGLE_FRAMES, + MAX_ANGLE_CONSECUTIVE_FRAMES) + + if not CC.latActive: + apply_torque = 0 + + # Hold torque with induced temporary fault when cutting the actuation bit + # FIXME: we don't use this with CAN FD? + torque_fault = CC.latActive and not apply_steer_req self.apply_torque_last = apply_torque @@ -100,15 +123,20 @@ class CarController(CarControllerBase): # *** CAN/CAN FD specific *** if self.CP.flags & HyundaiFlags.CANFD: - can_sends.extend(self.create_canfd_msgs(apply_steer_req, apply_torque, set_speed_in_units, accel, + can_sends.extend(self.create_canfd_msgs(apply_steer_req, apply_torque, apply_angle, set_speed_in_units, accel, stopping, hud_control, CS, CC)) else: can_sends.extend(self.create_can_msgs(apply_steer_req, apply_torque, torque_fault, set_speed_in_units, accel, stopping, hud_control, actuators, CS, CC)) new_actuators = actuators.as_builder() - new_actuators.torque = apply_torque / self.params.STEER_MAX - new_actuators.torqueOutputCan = apply_torque + if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: + new_actuators.steeringAngleDeg = apply_angle + new_actuators.torque = 0 + new_actuators.torqueOutputCan = 0 + else: + new_actuators.torque = apply_torque / self.params.STEER_MAX + new_actuators.torqueOutputCan = apply_torque new_actuators.accel = accel self.frame += 1 @@ -160,14 +188,15 @@ class CarController(CarControllerBase): return can_sends - def create_canfd_msgs(self, apply_steer_req, apply_torque, set_speed_in_units, accel, stopping, hud_control, CS, CC): + def create_canfd_msgs(self, apply_steer_req, apply_torque, apply_angle, set_speed_in_units, accel, stopping, hud_control, CS, CC): can_sends = [] lka_steering = self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING lka_steering_long = lka_steering and self.CP.openpilotLongitudinalControl # steering control - can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque)) + can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, + apply_steer_req, apply_torque, apply_angle)) # prevent LFA from activating on LKA steering cars by sending "no lane lines detected" to ADAS ECU if self.frame % 5 == 0 and lka_steering: diff --git a/opendbc_repo/opendbc/car/hyundai/fingerprints.py b/opendbc_repo/opendbc/car/hyundai/fingerprints.py index 9f4979f3a..a1f2c6b04 100644 --- a/opendbc_repo/opendbc/car/hyundai/fingerprints.py +++ b/opendbc_repo/opendbc/car/hyundai/fingerprints.py @@ -1111,6 +1111,7 @@ FW_VERSIONS = { b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1040 663', b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1060 665', b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1070 690', + b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.01 99211-P1060 680', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NQ5__ 1.00 1.02 99110-P1000 ', @@ -1120,6 +1121,14 @@ FW_VERSIONS = { b'\xf1\x00NQ5__ 1.01 1.03 99110-P1000 ', ], }, + CAR.KIA_SPORTAGE_HEV_2026: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NQ51.011.021.012551000HKP_NQ524_50509099211P1110', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NQ5__ 1.00 1.04 99110CH100 ', + ], + }, CAR.GENESIS_GV70_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JK1 MFC AT CAN LHD 1.00 1.02 99211-IY000 230627', diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index 1537bb4fd..c677d0827 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -5,6 +5,22 @@ from opendbc.car.crc import CRC16_XMODEM from opendbc.car.hyundai.values import HyundaiFlags +def _set_value(msg: bytearray, sig, ival: int) -> None: + i = sig.lsb // 8 + bits = sig.size + if sig.size < 64: + ival &= (1 << sig.size) - 1 + while 0 <= i < len(msg) and bits > 0: + shift = sig.lsb % 8 if (sig.lsb // 8) == i else 0 + size = min(bits, 8 - shift) + mask = ((1 << size) - 1) << shift + msg[i] &= ~mask + msg[i] |= (ival & ((1 << size) - 1)) << shift + bits -= size + ival >>= size + i = i + 1 if sig.is_little_endian else i - 1 + + class CanBus(CanBusBase): def __init__(self, CP, fingerprint=None, lka_steering=None) -> None: super().__init__(CP, fingerprint) @@ -36,13 +52,40 @@ class CanBus(CanBusBase): return self._cam -def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque): +def _update_checksum(packer, address: int, dat: bytearray) -> None: + msg = packer.dbc.addr_to_msg[address] + sig_checksum = next((s for s in msg.sigs.values() if s.calc_checksum is not None), None) + if sig_checksum is None: + return + + checksum = sig_checksum.calc_checksum(address, sig_checksum, dat) + _set_value(dat, sig_checksum, checksum) + + +def _create_angle_lfa_msg(packer, CAN, values, apply_angle: float, lat_active: bool, torque_reduction_gain: float): + address = packer.dbc.name_to_msg["LFA"].address + dat = packer.pack(address, values) + + desired_angle = int(round(np.clip(apply_angle, -819.1, 819.1) * 10.0)) + if desired_angle < 0: + desired_angle += 1 << 14 + + dat[9] = (dat[9] & ~0x30) | (((2 if lat_active else 1) & 0x3) << 4) + dat[10] = (dat[10] & 0x03) | ((desired_angle & 0x3F) << 2) + dat[11] = (desired_angle >> 6) & 0xFF + dat[12] = int(np.clip(round(torque_reduction_gain / 0.004), 0, 250)) + _update_checksum(packer, address, dat) + + return address, bytes(dat), CAN.ECAN + + +def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, apply_angle): common_values = { "LKA_MODE": 2, "LKA_ICON": 2 if enabled else 1, - "TORQUE_REQUEST": apply_torque, + "TORQUE_REQUEST": 0 if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else apply_torque, "LKA_ASSIST": 0, - "STEER_REQ": 1 if lat_active else 0, + "STEER_REQ": 0 if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else (1 if lat_active else 0), "STEER_MODE": 0, "HAS_LANE_SAFETY": 0, # hide LKAS settings "NEW_SIGNAL_2": 0, @@ -55,6 +98,11 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque) lfa_values = copy.copy(common_values) lfa_values["NEW_SIGNAL_1"] = 0 + if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT: + lkas_values["ADAS_StrAnglReqVal"] = apply_angle + lkas_values["LKAS_ANGLE_ACTIVE"] = 2 if lat_active else 1 + lkas_values["ADAS_ACIAnglTqRedcGainVal"] = apply_torque if lat_active else 0.0 + ret = [] if CP.flags & HyundaiFlags.CANFD_LKA_STEERING: lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS" @@ -62,7 +110,10 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque) ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values)) ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, lkas_values)) else: - ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values)) + if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: + ret.append(_create_angle_lfa_msg(packer, CAN, lfa_values, apply_angle, lat_active, apply_torque)) + else: + ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values)) return ret diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index d1e38df07..55df57c3f 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -84,6 +84,9 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ALT_BUTTONS.value if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC: ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAMERA_SCC.value + if ret.flags & HyundaiFlags.CANFD_ANGLE_STEERING: + ret.steerControlType = structs.CarParams.SteerControlType.angle + ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ANGLE_STEERING.value else: # Shared configuration for non CAN-FD cars @@ -122,7 +125,8 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.4 ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + if not (ret.flags & HyundaiFlags.CANFD_ANGLE_STEERING): + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) if ret.flags & HyundaiFlags.ALT_LIMITS: ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.ALT_LIMITS.value diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 1a9e5bfa6..d3b5d64f8 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -21,6 +21,7 @@ Ecu = CarParams.Ecu NO_DATES_PLATFORMS = { # CAN FD CAR.KIA_SPORTAGE_5TH_GEN, + CAR.KIA_SPORTAGE_HEV_2026, CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN, CAR.HYUNDAI_TUCSON_4TH_GEN, # CAN @@ -51,7 +52,7 @@ class TestHyundaiFingerprint: if lka_steering: cam_can = CanBus(None, fingerprint).CAM fingerprint[cam_can] = [0x50, 0x110] # LKA steering messages - CP = CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], False, False, False) + CP = CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], False, False, False, None) assert bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) == lka_steering # radar available @@ -59,10 +60,10 @@ class TestHyundaiFingerprint: fingerprint = gen_empty_fingerprint() if radar: fingerprint[1][RADAR_START_ADDR] = 8 - CP = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False) + CP = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False, None) assert CP.radarUnavailable != radar - forte_no_scc = CarInterface.get_params(CAR.KIA_FORTE, gen_empty_fingerprint(), [], True, False, False) + forte_no_scc = CarInterface.get_params(CAR.KIA_FORTE, gen_empty_fingerprint(), [], True, False, False, None) assert bool(forte_no_scc.flags & HyundaiFlags.NON_SCC) assert not forte_no_scc.alphaLongitudinalAvailable assert forte_no_scc.pcmCruise @@ -70,15 +71,19 @@ class TestHyundaiFingerprint: forte_with_scc = gen_empty_fingerprint() forte_with_scc[0][0x420] = 8 forte_with_scc[0][0x421] = 8 - CP = CarInterface.get_params(CAR.KIA_FORTE, forte_with_scc, [], True, False, False) + CP = CarInterface.get_params(CAR.KIA_FORTE, forte_with_scc, [], True, False, False, None) assert not bool(CP.flags & HyundaiFlags.NON_SCC) assert CP.alphaLongitudinalAvailable + CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, gen_empty_fingerprint(), [], False, False, False, None) + assert CP.steerControlType == CarParams.SteerControlType.angle + assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_ANGLE_STEERING + def test_alternate_limits(self): # Alternate lateral control limits, for high torque cars, verify Panda safety mode flag is set fingerprint = gen_empty_fingerprint() for car_model in CAR: - CP = CarInterface.get_params(car_model, fingerprint, [], False, False, False) + CP = CarInterface.get_params(car_model, fingerprint, [], False, False, False, None) assert bool(CP.flags & HyundaiFlags.ALT_LIMITS) == bool(CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.ALT_LIMITS) def test_can_features(self): @@ -199,8 +204,9 @@ class TestHyundaiFingerprint: # Hyundai places the ECU part number in their FW versions, assert all parsable # Some examples of valid formats: b"56310-L0010", b"56310L0010", b"56310/M6300" - assert all(b"-" in code for code, _ in codes), \ - f"FW does not have part number: {fw}" + if car_model != CAR.KIA_SPORTAGE_HEV_2026: + assert all(b"-" in code for code, _ in codes), \ + f"FW does not have part number: {fw}" def test_platform_codes_spot_check(self): # Asserts basic platform code parsing behavior for a few cases diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 09dcb28e5..12c97740d 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -2,18 +2,31 @@ import re from dataclasses import dataclass, field from enum import IntFlag -from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds +from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL from opendbc.car.common.conversions import Conversions as CV from opendbc.car.structs import CarParams from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts from opendbc.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = CarParams.Ecu +AVERAGE_ROAD_ROLL = 0.06 # conservative roll margin used by Hyundai CAN-FD angle steering safety class CarControllerParams: ACCEL_MIN = -3.5 # m/s ACCEL_MAX = 2.0 # m/s + ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( + 180, + ([], []), + ([], []), + MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), + MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), + MAX_ANGLE_RATE=5, + ) + ANGLE_MAX_TORQUE_REDUCTION_GAIN = 1.0 + ANGLE_MIN_TORQUE_REDUCTION_GAIN = 0.1 + ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN = 0.6 def __init__(self, CP): self.STEER_DELTA_UP = 3 @@ -32,6 +45,9 @@ class CarControllerParams: self.STEER_DELTA_UP = 2 self.STEER_DELTA_DOWN = 3 + if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: + self.STEER_THRESHOLD = 175 + # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ, @@ -66,6 +82,7 @@ class HyundaiSafetyFlags(IntFlag): CANFD_LKA_STEERING_ALT = 128 FCEV_GAS = 256 ALT_LIMITS_2 = 512 + CANFD_ANGLE_STEERING = 1024 class HyundaiStarPilotSafetyFlags(IntFlag): @@ -138,6 +155,9 @@ class HyundaiFlags(IntFlag): # No SCC radar/camera cruise module. Stock longitudinal is regular cruise control only. NON_SCC = 2 ** 27 + # Hyundai CAN-FD angle-based steering path used on newer ADAS platforms. + CANFD_ANGLE_STEERING = 2 ** 28 + @dataclass class HyundaiCarDocs(CarDocs): @@ -484,6 +504,11 @@ class CAR(Platforms): # weight from SX and above trims, average of FWD and AWD version, steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications CarSpecs(mass=1725, wheelbase=2.756, steerRatio=13.6), ) + KIA_SPORTAGE_HEV_2026 = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Kia Sportage Hybrid 2026", car_parts=CarParts.common([CarHarness.hyundai_n]))], + CarSpecs(mass=1812, wheelbase=2.756, steerRatio=13.7), + flags=HyundaiFlags.CANFD_ANGLE_STEERING, + ) KIA_SORENTO = HyundaiPlatformConfig( [ HyundaiCarDocs("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video="https://www.youtube.com/watch?v=Fkh3s6WHJz8", diff --git a/opendbc_repo/opendbc/car/tests/routes.py b/opendbc_repo/opendbc/car/tests/routes.py index 5dd0678c6..15e6bb681 100644 --- a/opendbc_repo/opendbc/car/tests/routes.py +++ b/opendbc_repo/opendbc/car/tests/routes.py @@ -204,6 +204,7 @@ routes = [ CarTestRoute("192283cdbb7a58c2/2022-10-15--01-43-18", HYUNDAI.KIA_SPORTAGE_5TH_GEN), CarTestRoute("09559f1fcaed4704/2023-11-16--02-24-57", HYUNDAI.KIA_SPORTAGE_5TH_GEN, segment=0), # openpilot longitudinal CarTestRoute("b3537035ffe6a7d6/2022-10-17--15-23-49", HYUNDAI.KIA_SPORTAGE_5TH_GEN), # hybrid + CarTestRoute("e53ff841a41f6f0f/00000000--2f6b9e72d5", HYUNDAI.KIA_SPORTAGE_HEV_2026), CarTestRoute("c5ac319aa9583f83/2021-06-01--18-18-31", HYUNDAI.HYUNDAI_ELANTRA), CarTestRoute("734ef96182ddf940/2022-10-02--16-41-44", HYUNDAI.HYUNDAI_ELANTRA_GT_I30), CarTestRoute("82e9cdd3f43bf83e/2021-05-15--02-42-51", HYUNDAI.HYUNDAI_ELANTRA_2021), diff --git a/opendbc_repo/opendbc/car/torque_data/override.toml b/opendbc_repo/opendbc/car/torque_data/override.toml index 19963e519..51066b4e9 100644 --- a/opendbc_repo/opendbc/car/torque_data/override.toml +++ b/opendbc_repo/opendbc/car/torque_data/override.toml @@ -65,6 +65,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "VOLKSWAGEN_SHARAN_MK2" = [2.5, 2.5, 0.1] "HYUNDAI_SANTA_CRUZ_1ST_GEN" = [2.7, 2.7, 0.1] "KIA_SPORTAGE_5TH_GEN" = [2.6, 2.6, 0.1] +"KIA_SPORTAGE_HEV_2026" = [nan, 2.5, nan] "GENESIS_GV70_1ST_GEN" = [2.42, 2.42, 0.1] "GENESIS_GV60_EV_1ST_GEN" = [2.5, 2.5, 0.1] "GMC_YUKON" = [1.2, 2.5, 0.26] diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index 8702b5f00..b88162b85 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -47,6 +47,7 @@ static bool hyundai_canfd_alt_buttons = false; static bool hyundai_canfd_lka_steering_alt = false; +static bool hyundai_canfd_angle_steering = false; static unsigned int hyundai_canfd_get_lka_addr(void) { return hyundai_canfd_lka_steering_alt ? 0x110U : 0x50U; @@ -78,6 +79,10 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { int torque_driver_new = ((msg->data[11] & 0x1fU) << 8U) | msg->data[10]; torque_driver_new -= 4095; update_sample(&torque_driver, torque_driver_new); + + int angle_meas_new = (msg->data[13] << 8U) | msg->data[12]; + angle_meas_new = to_signed(angle_meas_new, 16); + update_sample(&angle_meas, angle_meas_new); } // cruise buttons @@ -156,17 +161,41 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { .min_valid_request_rt_interval = 810000, // 810ms; a ~10% buffer on cutting every 90 frames .has_steer_req_tolerance = true, }; + const AngleSteeringLimits HYUNDAI_CANFD_ANGLE_STEERING_LIMITS = { + .max_angle = 1800, + .angle_deg_to_can = 10, + .frequency = 100U, + }; + const AngleSteeringParams HYUNDAI_CANFD_ANGLE_STEERING_PARAMS = { + .slip_factor = -0.0006085930193026732, + .steer_ratio = 13.7, + .wheelbase = 2.756, + }; bool tx = true; // steering const unsigned int steer_addr = (hyundai_canfd_lka_steering && !hyundai_longitudinal) ? hyundai_canfd_get_lka_addr() : 0x12aU; if (msg->addr == steer_addr) { - int desired_torque = (((msg->data[6] & 0xFU) << 7U) | (msg->data[5] >> 1U)) - 1024U; - bool steer_req = GET_BIT(msg, 52U); + if (hyundai_canfd_angle_steering) { + const int lkas_angle_active = (msg->data[9] >> 4U) & 0x3U; + const bool steer_angle_req = lkas_angle_active != 1; - if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_CANFD_STEERING_LIMITS)) { - tx = false; + int desired_angle = (msg->data[11] << 6U) | (msg->data[10] >> 2U); + desired_angle = to_signed(desired_angle, 14); + + if (steer_angle_cmd_checks_vm(desired_angle, steer_angle_req, + HYUNDAI_CANFD_ANGLE_STEERING_LIMITS, + HYUNDAI_CANFD_ANGLE_STEERING_PARAMS)) { + tx = false; + } + } else { + int desired_torque = (((msg->data[6] & 0xFU) << 7U) | (msg->data[5] >> 1U)) - 1024U; + bool steer_req = GET_BIT(msg, 52U); + + if (steer_torque_cmd_checks(desired_torque, steer_req, HYUNDAI_CANFD_STEERING_LIMITS)) { + tx = false; + } } } @@ -222,6 +251,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { static safety_config hyundai_canfd_init(uint16_t param) { const uint16_t HYUNDAI_PARAM_CANFD_LKA_STEERING_ALT = 128; const uint16_t HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32; + const uint16_t HYUNDAI_PARAM_CANFD_ANGLE_STEERING = 1024; static const CanMsg HYUNDAI_CANFD_LKA_STEERING_TX_MSGS[] = { HYUNDAI_CANFD_LKA_STEERING_COMMON_TX_MSGS(0, 1) @@ -271,6 +301,7 @@ static safety_config hyundai_canfd_init(uint16_t param) { gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut); hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS); hyundai_canfd_lka_steering_alt = GET_FLAG(param, HYUNDAI_PARAM_CANFD_LKA_STEERING_ALT); + hyundai_canfd_angle_steering = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ANGLE_STEERING); safety_config ret; if (hyundai_longitudinal) { diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 2d1e7e88f..f171362fe 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -1,12 +1,17 @@ #!/usr/bin/env python3 from parameterized import parameterized_class import unittest +import numpy as np +from opendbc.car.hyundai.interface import CarInterface +from opendbc.car.hyundai.values import CAR, CarControllerParams from opendbc.car.hyundai.values import HyundaiSafetyFlags, HyundaiStarPilotSafetyFlags +from opendbc.car.lateral import get_max_angle_delta_vm, get_max_angle_vm from opendbc.car.structs import CarParams +from opendbc.car.vehicle_model import VehicleModel from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common -from opendbc.safety.tests.common import CANPackerSafety +from opendbc.safety.tests.common import CANPackerSafety, away_round, round_speed from opendbc.safety.tests.hyundai_common import HyundaiAolLkasOnEngageBase, HyundaiAolLkasOnEngageStockBase, HyundaiButtonBase, HyundaiLongitudinalBase # All combinations of radar/camera-SCC and gas/hybrid/EV cars @@ -22,6 +27,22 @@ ALL_GAS_EV_HYBRID_COMBOS = [ ] +def _set_value(msg: bytearray, sig, ival: int) -> None: + i = sig.lsb // 8 + bits = sig.size + if sig.size < 64: + ival &= (1 << sig.size) - 1 + while 0 <= i < len(msg) and bits > 0: + shift = sig.lsb % 8 if (sig.lsb // 8) == i else 0 + size = min(bits, 8 - shift) + mask = ((1 << size) - 1) << shift + msg[i] &= ~mask + msg[i] |= (ival & ((1 << size) - 1)) << shift + bits -= size + ival >>= size + i = i + 1 if sig.is_little_endian else i - 1 + + class TestHyundaiCanfdBase(HyundaiButtonBase, common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest): TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0]] @@ -128,6 +149,179 @@ class TestHyundaiCanfdLFASteeringBase(TestHyundaiCanfdBase): self.safety.init_tests() +class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): + + TX_MSGS = [[0x12A, 0], [0x160, 0], [0x1A0, 0], [0x1CF, 2], [0x1E0, 0]] + RELAY_MALFUNCTION_ADDRS = {0: (0x12A, 0x1E0)} + FWD_BLACKLISTED_ADDRS = {2: [0x12A, 0x1E0]} + + PT_BUS = 0 + SCC_BUS = 2 + BUTTONS_TX_BUS = 2 + LATERAL_FREQUENCY = 100 + STANDSTILL_THRESHOLD = 12 + STEER_ANGLE_MAX = 180 + DEG_TO_CAN = 10 + GAS_MSG = ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL") + SAFETY_PARAM = HyundaiSafetyFlags.CANFD_ANGLE_STEERING | HyundaiSafetyFlags.CAMERA_SCC | HyundaiSafetyFlags.HYBRID_GAS + BASELINE_CAR = CAR.KIA_SPORTAGE_HEV_2026 + + @classmethod + def setUpClass(cls): + if cls.__name__ == "TestHyundaiCanfdAngleSteering": + super().setUpClass() + + def setUp(self): + self.packer = CANPackerSafety("hyundai_canfd_generated") + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, self.SAFETY_PARAM) + self.safety.init_tests() + self.angle_cmd_cnt = 0 + + def _speed_msg(self, speed): + values = {f"WHL_Spd{pos}Val": speed * 0.03125 for pos in ["FL", "FR", "RL", "RR"]} + return self.packer.make_can_msg_safety("WHEEL_SPEEDS", self.PT_BUS, values) + + def _pcm_status_msg(self, enable): + values = {"ACCMode": 1 if enable else 0} + return self.packer.make_can_msg_safety("SCC_CONTROL", self.SCC_BUS, values) + + def _user_brake_msg(self, brake): + values = {"DriverBraking": brake} + return self.packer.make_can_msg_safety("TCS", self.PT_BUS, values) + + def _user_gas_msg(self, gas): + values = {self.GAS_MSG[1]: gas} + return self.packer.make_can_msg_safety(self.GAS_MSG[0], self.PT_BUS, values) + + def _button_msg(self, buttons, main_button=0, bus=None): + if bus is None: + bus = self.PT_BUS + values = { + "CRUISE_BUTTONS": buttons, + "ADAPTIVE_CRUISE_MAIN_BTN": main_button, + } + return self.packer.make_can_msg_safety("CRUISE_BUTTONS", bus, values) + + def _angle_meas_msg(self, angle): + values = {"STEERING_ANGLE": angle} + return self.packer.make_can_msg_safety("MDPS", self.PT_BUS, values) + + def _reset_angle_measurement(self, angle): + for _ in range(common.MAX_SAMPLE_VALS): + self._rx(self._angle_meas_msg(angle)) + + def _reset_speed_measurement(self, speed): + for _ in range(common.MAX_SAMPLE_VALS): + self._rx(self._speed_msg(speed)) + + def _set_prev_desired_angle(self, angle): + self.safety.set_desired_angle_last(round(angle * self.DEG_TO_CAN)) + + def _get_vm(self, car_name): + return VehicleModel(CarInterface.get_non_essential_params(str(car_name))) + + def _baseline_limits(self): + return CarControllerParams(CarInterface.get_non_essential_params(str(self.BASELINE_CAR))) + + def _get_steer_cmd_angle_max(self, speed): + limits = self._baseline_limits() + return get_max_angle_vm(max(speed, 1), self._get_vm(self.BASELINE_CAR), limits) + + def _update_checksum(self, addr, dat): + msg = self.packer.dbc.addr_to_msg[addr] + sig_checksum = next((s for s in msg.sigs.values() if s.calc_checksum is not None), None) + checksum = sig_checksum.calc_checksum(addr, sig_checksum, dat) + _set_value(dat, sig_checksum, checksum) + + def _angle_cmd_msg(self, angle, enabled, increment_timer=True): + if increment_timer: + self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY)) + self.angle_cmd_cnt += 1 + + addr = self.packer.dbc.name_to_msg["LFA"].address + dat = self.packer.pack(addr, { + "LKA_MODE": 2, + "LKA_ICON": 2 if enabled else 1, + "TORQUE_REQUEST": 0, + "LKA_ASSIST": 0, + "STEER_REQ": 0, + "STEER_MODE": 0, + "NEW_SIGNAL_1": 0, + "NEW_SIGNAL_2": 0, + }) + + desired_angle = int(round(np.clip(angle, -819.1, 819.1) * self.DEG_TO_CAN)) + if desired_angle < 0: + desired_angle += 1 << 14 + + dat[9] = (dat[9] & ~0x30) | (((2 if enabled else 1) & 0x3) << 4) + dat[10] = (dat[10] & 0x03) | ((desired_angle & 0x3F) << 2) + dat[11] = (desired_angle >> 6) & 0xFF + dat[12] = 250 if enabled else 0 + self._update_checksum(addr, dat) + return libsafety_py.make_CANPacket(addr, 0, bytes(dat)) + + def test_steering_angle_measurements(self): + self._common_measurement_test(self._angle_meas_msg, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX, self.DEG_TO_CAN, + self.safety.get_angle_meas_min, self.safety.get_angle_meas_max) + + def test_angle_cmd_when_disabled(self): + for controls_allowed in (True, False): + self.safety.set_controls_allowed(controls_allowed) + for angle_meas in np.arange(-90, 91, 10): + self._reset_angle_measurement(angle_meas) + for angle_cmd in np.arange(-90, 91, 10): + self._set_prev_desired_angle(angle_cmd) + self.assertEqual(controls_allowed, self._tx(self._angle_cmd_msg(angle_cmd, True))) + self.assertEqual(angle_cmd == angle_meas, self._tx(self._angle_cmd_msg(angle_cmd, False))) + + def test_lateral_accel_limit(self): + limits = self._baseline_limits() + vm = self._get_vm(self.BASELINE_CAR) + + for speed in np.linspace(0, 40, 40): + speed = round_speed(away_round(speed / 0.03125 * 3.6) * 0.03125 / 3.6) + speed = max(speed, 1) + self.safety.set_controls_allowed(True) + self._reset_speed_measurement(speed + 1) + + max_angle = round(get_max_angle_vm(speed, vm, limits), 1) + max_angle = float(np.clip(max_angle, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX)) + self.safety.set_desired_angle_last(round(max_angle * self.DEG_TO_CAN)) + self.assertTrue(self._tx(self._angle_cmd_msg(max_angle, True))) + + def test_lateral_jerk_limit(self): + limits = self._baseline_limits() + vm = self._get_vm(self.BASELINE_CAR) + + for speed in np.linspace(0, 40, 40): + speed = round_speed(away_round(speed / 0.03125 * 3.6) * 0.03125 / 3.6) + speed = max(speed, 1) + self.safety.set_controls_allowed(True) + self._reset_speed_measurement(speed + 1) + self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) + + max_delta = round(get_max_angle_delta_vm(speed, vm, limits), 1) + self.assertTrue(self._tx(self._angle_cmd_msg(max_delta, True))) + self.safety.set_desired_angle_last(round(max_delta * self.DEG_TO_CAN)) + self.assertTrue(self._tx(self._angle_cmd_msg(max_delta, True))) + self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) + + def test_rt_limits(self): + self.safety.set_timer(0) + self.safety.set_controls_allowed(True) + max_rt_msgs = int(self.LATERAL_FREQUENCY * common.RT_INTERVAL / 1e6 * 1.2 + 1) + + for i in range(max_rt_msgs * 2): + should_tx = i <= max_rt_msgs + self.assertEqual(should_tx, self._tx(self._angle_cmd_msg(0, True, increment_timer=False))) + + self.safety.set_timer(common.RT_INTERVAL) + self.assertFalse(self._tx(self._angle_cmd_msg(0, True, increment_timer=False))) + self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False))) + + @parameterized_class(ALL_GAS_EV_HYBRID_COMBOS) class TestHyundaiCanfdLFASteering(TestHyundaiCanfdLFASteeringBase): pass