mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-03 12:32:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

add kia sportage hybrid support

Browse Source
This commit is contained in:
firestar5683
2026-04-22 17:44:22 -05:00
parent 7c71f3c114
commit c36235f6fd
11 changed files with 389 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/fingerprints.py
+2
View File
@@ -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,
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+47 -18
View File
@@ -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:
opendbc_repo/opendbc/car/hyundai/fingerprints.py
+9
View File
@@ -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',
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+55 -4
View File
@@ -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
opendbc_repo/opendbc/car/hyundai/interface.py
+5 -1
View File
@@ -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
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+13 -7
View File
@@ -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
opendbc_repo/opendbc/car/hyundai/values.py
+26 -1
View File
@@ -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",
opendbc_repo/opendbc/car/tests/routes.py
+1
View File
@@ -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),
opendbc_repo/opendbc/car/torque_data/override.toml
+1
View File
@@ -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]
opendbc_repo/opendbc/safety/modes/hyundai_canfd.h
+35 -4
View File
@@ -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) {
opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py
+195 -1
View File
@@ -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