pan131419/openpilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Release 260111

Browse Source
This commit is contained in:
Comma Device
2026-01-11 18:23:29 +08:00
commit 3721ecbf8a
2601 changed files with 855070 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
opendbc_repo/opendbc/car/hyundai/__init__.py Normal file
View File

637
opendbc_repo/opendbc/car/hyundai/carcontroller.py Normal file
View File

@@ -0,0 +1,637 @@
import numpy as np
from opendbc.can import CANPacker
from opendbc.car import Bus, DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg, structs, apply_std_steer_angle_limits
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.hyundai import hyundaicanfd, hyundaican
from opendbc.car.hyundai.carstate import CarState
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CAR, CAN_GEARS, HyundaiExtFlags
from opendbc.car.interfaces import CarControllerBase
VisualAlert = structs.CarControl.HUDControl.VisualAlert
LongCtrlState = structs.CarControl.Actuators.LongControlState
from openpilot.common.params import Params
# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
# All slightly below EPS thresholds to avoid fault
MAX_ANGLE = 85
MAX_ANGLE_FRAMES = 89
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
vibrate_intervals = [
(0.0, 0.5),
(1.0, 1.5),
#(2.5, 3.0),
#(3.5, 4.0),
(5.0, 5.5),
(6.0, 6.5),
(7.5, 8.0),
]
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
# initialize to no line visible
# TODO: this is not accurate for all cars
sys_state = 1
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
sys_state = 3 if enabled or sys_warning else 4
elif hud_control.leftLaneVisible:
sys_state = 5
elif hud_control.rightLaneVisible:
sys_state = 6
# initialize to no warnings
left_lane_warning = 0
right_lane_warning = 0
if hud_control.leftLaneDepart:
left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
if hud_control.rightLaneDepart:
right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
return sys_warning, sys_state, left_lane_warning, right_lane_warning
def calc_rate_limit_by_lat_accel(delta_deg: float,
v_ego: float,
wheelbase: float,
max_lat_accel: float,
max_rate_low: float,
max_rate_high: float) -> float:
"""
반환값: 허용 스티어링휠 각속도 (deg/s)
delta_deg : 현재 스티어링휠 각도 (deg)
v_ego : 속도 (m/s)
wheelbase : 축거 (m)
"""
# v가 너무 작으면 공식이 터지니까, 저속 전용 상수 사용
if v_ego < 0.5:
return max_rate_low # 예: 300 deg/s
delta_rad = np.radians(delta_deg)
# cos^2 항이 0에 가까워지면 rate가 폭발하니 하한 넣어줌
cos_delta = np.cos(delta_rad)
cos2 = max(cos_delta * cos_delta, 0.05) # 0.05 정도면 충분
# 물리식: |δ̇| <= L * a_lat_max / (v^2 * sec^2(δ))
# 여기서 sec^2(δ) = 1 / cos^2(δ)
delta_rate_rad_s = (wheelbase * max_lat_accel) / (v_ego * v_ego * cos2)
delta_rate_deg_s = np.degrees(delta_rate_rad_s)
# 저속에서는 너무 크지 않게, 고속에서는 너무 작지 않게 clip
# max_rate_high <= delta_rate_deg_s <= max_rate_low
return float(np.clip(delta_rate_deg_s, max_rate_high, max_rate_low))
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP):
super().__init__(dbc_names, CP)
self.CAN = CanBus(CP)
self.params = CarControllerParams(CP)
self.packer = CANPacker(dbc_names[Bus.pt])
self.angle_limit_counter = 0
self.accel_last = 0
self.apply_torque_last = 0
self.car_fingerprint = CP.carFingerprint
self.last_button_frame = 0
self.hyundai_jerk = HyundaiJerk()
self.speedCameraHapticEndFrame = 0
self.hapticFeedbackWhenSpeedCamera = 0
self.max_angle_frames = MAX_ANGLE_FRAMES
self.blinking_signal = False # 1Hz
self.blinking_frame = int(1.0 / DT_CTRL)
self.soft_hold_mode = 2
self.activateCruise = 0
self.button_wait = 12
self.cruise_buttons_msg_values = None
self.cruise_buttons_msg_cnt = 0
self.button_spamming_count = 0
self.prev_clu_speed = 0
self.button_spam1 = 8
self.button_spam2 = 30
self.button_spam3 = 1
self.apply_angle_last = 0
self.lkas_max_torque = 0
self.angle_max_torque = 250
self.canfd_debug = 0
self.MainMode_ACC_trigger = 0
self.LFA_trigger = 0
self.activeCarrot = 0
self.camera_scc_params = Params().get_int("HyundaiCameraSCC")
self.is_ldws_car = Params().get_bool("IsLdwsCar")
self.steerDeltaUpOrg = self.steerDeltaUp = self.steerDeltaUpLC = self.params.STEER_DELTA_UP
self.steerDeltaDownOrg = self.steerDeltaDown = self.steerDeltaDownLC = self.params.STEER_DELTA_DOWN
def update(self, CC, CS, now_nanos):
if self.frame % 50 == 0:
params = Params()
self.max_angle_frames = params.get_int("MaxAngleFrames")
steerMax = params.get_int("CustomSteerMax")
steerDeltaUp = params.get_int("CustomSteerDeltaUp")
steerDeltaDown = params.get_int("CustomSteerDeltaDown")
steerDeltaUpLC = params.get_int("CustomSteerDeltaUpLC")
steerDeltaDownLC = params.get_int("CustomSteerDeltaDownLC")
if steerMax > 0:
self.params.STEER_MAX = steerMax
if steerDeltaUp > 0:
self.steerDeltaUp = steerDeltaUp
#self.params.ANGLE_TORQUE_UP_RATE = steerDeltaUp
else:
self.steerDeltaUp = self.steerDeltaUpOrg
if steerDeltaDown > 0:
self.steerDeltaDown = steerDeltaDown
#self.params.ANGLE_TORQUE_DOWN_RATE = steerDeltaDown
else:
self.steerDeltaDown = self.steerDeltaDownOrg
if steerDeltaUpLC > 0:
self.steerDeltaUpLC = steerDeltaUpLC
else:
self.steerDeltaUpLC = self.steerDeltaUp
if steerDeltaDownLC > 0:
self.steerDeltaDownLC = steerDeltaDownLC
else:
self.steerDeltaDownLC = self.steerDeltaDown
self.soft_hold_mode = 1 if params.get_int("AutoCruiseControl") > 1 else 2
self.hapticFeedbackWhenSpeedCamera = int(params.get_int("HapticFeedbackWhenSpeedCamera"))
self.button_spam1 = params.get_int("CruiseButtonTest1")
self.button_spam2 = params.get_int("CruiseButtonTest2")
self.button_spam3 = params.get_int("CruiseButtonTest3")
self.speed_from_pcm = params.get_int("SpeedFromPCM")
self.canfd_debug = params.get_int("CanfdDebug")
self.camera_scc_params = params.get_int("HyundaiCameraSCC")
actuators = CC.actuators
hud_control = CC.hudControl
if hud_control.modelDesire in [3,4]:
self.params.STEER_DELTA_UP = self.steerDeltaUpLC
self.params.STEER_DELTA_DOWN = self.steerDeltaDownLC
else:
self.params.STEER_DELTA_UP = self.steerDeltaUp
self.params.STEER_DELTA_DOWN = self.steerDeltaDown
angle_control = self.CP.flags & HyundaiFlags.ANGLE_CONTROL
# 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, self.max_angle_frames,
MAX_ANGLE_CONSECUTIVE_FRAMES)
#apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw,
# CS.out.steeringAngleDeg, CC.latActive, self.params.ANGLE_LIMITS)
MAX_LAT_ACCEL = 8.0
MAX_RATE_LOW = 200 # 저속, deg/s
MAX_RATE_HIGH = 40 # 고속, deg/s
UNWIND_SCALE = 1.5
UNWIND_MAX = 200
delta = actuators.steeringAngleDeg - self.apply_angle_last
same_dir = (np.sign(delta) == np.sign(self.apply_angle_last)) or (abs(self.apply_angle_last) < 2.0)
rate_deg_s = calc_rate_limit_by_lat_accel(self.apply_angle_last, CS.out.vEgoRaw, self.CP.wheelbase, MAX_LAT_ACCEL, MAX_RATE_LOW, MAX_RATE_HIGH)
if not same_dir:
rate_deg_s = min(rate_deg_s * UNWIND_SCALE, UNWIND_MAX)
rate_deg_per_tick = rate_deg_s * DT_CTRL
apply_angle = np.clip(actuators.steeringAngleDeg,
self.apply_angle_last - rate_deg_per_tick,
self.apply_angle_last + rate_deg_per_tick)
angle_limits = self.params.ANGLE_LIMITS
apply_angle = np.clip(apply_angle, -angle_limits.STEER_ANGLE_MAX, angle_limits.STEER_ANGLE_MAX)
#if abs(apply_angle - self.apply_angle_last) > 0.1:
# alpha = min(0.1 + 0.9 * CS.out.vEgoRaw / (30.0 * CV.KPH_TO_MS), 1.0)
# apply_angle = self.apply_angle_last * (1 - alpha) + apply_angle * alpha
v_ego_kph = CS.out.vEgoRaw * CV.MS_TO_KPH
if abs(apply_angle - self.apply_angle_last) < 0.1:
alpha = min(0.05 + 0.45 * v_ego_kph / 30.0, 0.5)
else:
alpha = 1.0 # min(0.1 + 0.9 * v_ego_kph / 30.0, 1.0)
apply_angle = self.apply_angle_last * (1 - alpha) + apply_angle * alpha
if angle_control:
apply_steer_req = CC.latActive
if CS.out.steeringPressed:
#self.apply_angle_last = CS.out.steeringAngleDeg
self.lkas_max_torque = self.lkas_max_torque = max(self.lkas_max_torque - 20, 25)
else:
target_torque = self.angle_max_torque
max_steering_tq = self.params.STEER_DRIVER_ALLOWANCE * 0.7
rate_ratio = max(20, max_steering_tq - abs(CS.out.steeringTorque)) / max_steering_tq
rate_up = self.params.ANGLE_TORQUE_UP_RATE * rate_ratio
rate_down = self.params.ANGLE_TORQUE_DOWN_RATE * rate_ratio
if self.lkas_max_torque > target_torque:
self.lkas_max_torque = max(self.lkas_max_torque - rate_down, target_torque)
else:
self.lkas_max_torque = min(self.lkas_max_torque + rate_up, target_torque)
if not CC.latActive:
apply_torque = 0
self.lkas_max_torque = 0
self.apply_angle_last = apply_angle
# Hold torque with induced temporary fault when cutting the actuation bit
torque_fault = CC.latActive and not apply_steer_req
self.apply_torque_last = apply_torque
# accel + longitudinal
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
active_speed_decel = hud_control.activeCarrot == 3 and self.activeCarrot != 3 # 3: Speed Decel
self.activeCarrot = hud_control.activeCarrot
if active_speed_decel and self.speedCameraHapticEndFrame < 0: # 과속카메라 감속시작
self.speedCameraHapticEndFrame = self.frame + (8.0 / DT_CTRL) #8초간 켜줌.
elif not active_speed_decel:
self.speedCameraHapticEndFrame = -1
if 0 <= self.speedCameraHapticEndFrame - self.frame < int(8.0 / DT_CTRL) and self.hapticFeedbackWhenSpeedCamera > 0:
t = (self.frame - (self.speedCameraHapticEndFrame - int(8.0 / DT_CTRL))) * DT_CTRL
for start, end in vibrate_intervals:
if start <= t < end:
left_lane_warning = right_lane_warning = self.hapticFeedbackWhenSpeedCamera
break
if self.frame >= self.speedCameraHapticEndFrame:
self.speedCameraHapticEndFrame = -1
if self.frame % self.blinking_frame == 0:
self.blinking_signal = True
elif self.frame % self.blinking_frame == self.blinking_frame / 2:
self.blinking_signal = False
can_sends = []
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, self.CAN.ECAN if self.CP.flags & HyundaiFlags.CANFD else 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, self.CAN.ECAN
can_sends.append(make_tester_present_msg(addr, bus, suppress_response=True))
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append(make_tester_present_msg(0x7b1, self.CAN.ECAN, suppress_response=True))
camera_scc = self.CP.flags & HyundaiFlags.CAMERA_SCC
# CAN-FD platforms
if self.CP.flags & HyundaiFlags.CANFD:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
if camera_scc:
can_sends.extend(hyundaicanfd.create_steering_messages_camera_scc(self.frame, self.packer, self.CP, self.CAN, CC, apply_steer_req, apply_torque, CS, apply_angle, self.lkas_max_torque, angle_control))
else:
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, apply_angle, self.lkas_max_torque, angle_control))
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
if self.frame % 5 == 0 and hda2 and not camera_scc:
can_sends.extend(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS))
# LFA and HDA icons
if self.frame % 5 == 0 and camera_scc:
can_sends.extend(hyundaicanfd.create_lfahda_cluster(self.packer, CS, self.CAN, CC.longActive, CC.latActive))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
if self.camera_scc_params in [2, 3]:
self.canfd_toggle_adas(CC, CS)
if self.CP.openpilotLongitudinalControl:
self.hyundai_jerk.make_jerk(self.CP, CS, accel, actuators, hud_control)
self.hyundai_jerk.check_carrot_cruise(CC, CS, hud_control, stopping, accel, actuators.aTarget)
if True: #not camera_scc:
can_sends.extend(hyundaicanfd.create_ccnc_messages(self.CP, self.packer, self.CAN, self.frame, CC, CS, hud_control, apply_angle, left_lane_warning, right_lane_warning, self.canfd_debug, self.MainMode_ACC_trigger, self.LFA_trigger))
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.CP, self.packer, self.CAN, self.frame))
else:
can_sends.extend(hyundaicanfd.create_fca_warning_light(self.CP, self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
if self.CP.flags & HyundaiFlags.CAMERA_SCC.value:
can_sends.append(hyundaicanfd.create_acc_control_scc2(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, hud_control, self.hyundai_jerk, CS))
can_sends.extend(hyundaicanfd.create_tcs_messages(self.packer, self.CAN, CS)) # for sorento SCC radar...
else:
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, hud_control, self.hyundai_jerk.jerk_u, self.hyundai_jerk.jerk_l, CS))
self.accel_last = accel
else:
# button presses
if self.camera_scc_params == 3: # camera scc but stock long
send_button = self.make_spam_button(CC, CS)
can_sends.extend(hyundaicanfd.forward_button_message(self.packer, self.CAN, self.frame, CS, send_button, self.MainMode_ACC_trigger, self.LFA_trigger))
else:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_torque, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning, self.is_ldws_car))
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
if self.CP.carFingerprint in CAN_GEARS["send_mdps12"]: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(hyundaican.create_mdps12(self.packer, self.frame, CS.mdps12))
casper_opt = self.CP.carFingerprint in (CAR.HYUNDAI_CASPER_EV)
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
self.hyundai_jerk.make_jerk(self.CP, CS, accel, actuators, hud_control)
self.hyundai_jerk.check_carrot_cruise(CC, CS, hud_control, stopping, accel, actuators.aTarget)
#jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
if camera_scc:
can_sends.extend(hyundaican.create_acc_commands_scc(self.packer, CC.enabled, accel, self.hyundai_jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, casper_opt, CS, self.soft_hold_mode))
else:
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, self.hyundai_jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, self.CP, CS, self.soft_hold_mode))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC, self.blinking_signal))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
if camera_scc:
if CS.scc13 is not None:
if casper_opt:
#can_sends.append(hyundaican.create_acc_opt_copy(CS, self.packer))
pass
pass
else:
can_sends.extend(hyundaican.create_acc_opt(self.packer, self.CP))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl and not camera_scc:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
new_actuators = actuators.as_builder()
new_actuators.torque = apply_torque / self.params.STEER_MAX
new_actuators.torqueOutputCan = apply_torque
new_actuators.steeringAngleDeg = float(apply_angle)
new_actuators.accel = accel
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: structs.CarControl, CS: CarState, use_clu11: bool):
can_sends = []
if CS.out.brakePressed or CS.out.brakeHoldActive:
return can_sends
if use_clu11:
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP))
elif False: #CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP)] * 25)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
if self.last_button_frame != self.frame:
send_button = self.make_spam_button(CC, CS)
if send_button > 0:
can_sends.append(hyundaican.create_clu11_button(self.packer, self.frame, CS.clu11, send_button, self.CP))
else:
# carrot.. 왜 alt_cruise_button는 값이 리스트일까?, 그리고 왜? 빈데이터가 들어오는것일까?
if CS.cruise_buttons_msg is not None and self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
try:
cruise_buttons_msg_values = {key: value[0] for key, value in CS.cruise_buttons_msg.items()}
except: # IndexError:
#print("IndexError....")
cruise_buttons_msg_values = None
self.cruise_buttons_msg_cnt += 1
if cruise_buttons_msg_values is not None:
self.cruise_buttons_msg_values = cruise_buttons_msg_values
self.cruise_buttons_msg_cnt = 0
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# cruise cancel
if CC.cruiseControl.cancel:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
print("cruiseControl.cancel222222")
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
#can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
if self.cruise_buttons_msg_values is not None:
can_sends.append(hyundaicanfd.alt_cruise_buttons(self.packer, self.CP, self.CAN, Buttons.CANCEL, self.cruise_buttons_msg_values, self.cruise_buttons_msg_cnt))
else:
for _ in range(20):
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
elif False: #CC.cruiseControl.resume:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# TODO: resume for alt button cars
pass
else:
for _ in range(20):
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
## button 스패밍을 안했을때...
if self.last_button_frame != self.frame:
dat = self.canfd_speed_control_pcm(CC, CS, self.cruise_buttons_msg_values)
if dat is not None:
for _ in range(self.button_spam3):
can_sends.append(dat)
self.cruise_buttons_msg_cnt += 1
return can_sends
def canfd_toggle_adas(self, CC, CS):
trigger_min = -200
trigger_start = 6
self.MainMode_ACC_trigger = max(trigger_min, self.MainMode_ACC_trigger - 1)
self.LFA_trigger = max(trigger_min, self.LFA_trigger - 1)
if self.MainMode_ACC_trigger == trigger_min and self.LFA_trigger == trigger_min:
if CC.enabled and not CS.MainMode_ACC and CS.out.vEgo > 3.:
self.MainMode_ACC_trigger = trigger_start
elif CC.latActive and CS.LFA_ICON == 0:
self.LFA_trigger = trigger_start
def canfd_speed_control_pcm(self, CC, CS, cruise_buttons_msg_values):
alt_buttons = True if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else False
if alt_buttons and cruise_buttons_msg_values is None:
return None
send_button = self.make_spam_button(CC, CS)
if send_button > 0:
if alt_buttons:
return hyundaicanfd.alt_cruise_buttons(self.packer, self.CP, self.CAN, send_button, cruise_buttons_msg_values, self.cruise_buttons_msg_cnt)
else:
return hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, send_button)
return None
def make_spam_button(self, CC, CS):
hud_control = CC.hudControl
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
target = int(set_speed_in_units+0.5)
current = int(CS.out.cruiseState.speed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) + 0.5)
v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH
send_button = 0
activate_cruise = False
if CC.enabled:
if not CS.out.cruiseState.enabled:
if (hud_control.leadVisible or v_ego_kph > 10.0) and self.activateCruise == 0:
send_button = Buttons.RES_ACCEL
self.activateCruise = 1
activate_cruise = True
elif CC.cruiseControl.resume:
send_button = Buttons.RES_ACCEL
elif target < current and current>= 31 and self.speed_from_pcm != 1:
send_button = Buttons.SET_DECEL
elif target > current and current < 160 and self.speed_from_pcm != 1:
send_button = Buttons.RES_ACCEL
elif CS.out.activateCruise: #CC.cruiseControl.activate:
if (hud_control.leadVisible or v_ego_kph > 10.0) and self.activateCruise == 0:
self.activateCruise = 1
send_button = Buttons.RES_ACCEL
activate_cruise = True
if CS.out.brakePressed or CS.out.gasPressed:
self.activateCruise = 0
if send_button == 0:
self.button_spamming_count = 0
self.prev_clu_speed = current
return 0
speed_diff = self.prev_clu_speed - current
spamming_max = self.button_spam1
if CS.cruise_buttons[-1] != Buttons.NONE:
self.last_button_frame = self.frame
self.button_wait = self.button_spam2
self.button_spamming_count = 0
elif abs(self.button_spamming_count) >= spamming_max or abs(speed_diff) > 0:
self.last_button_frame = self.frame
self.button_wait = self.button_spam2 if abs(self.button_spamming_count) >= spamming_max else 7
self.button_spamming_count = 0
self.prev_clu_speed = current
send_button_allowed = (self.frame - self.last_button_frame) > self.button_wait
#CC.debugTextCC = "{} speed_diff={:.1f},{:.0f}/{:.0f}, button={}, button_wait={}, count={}".format(
# send_button_allowed, speed_diff, target, current, send_button, self.button_wait, self.button_spamming_count)
if send_button_allowed or activate_cruise or (CC.cruiseControl.resume and self.frame % 2 == 0):
self.button_spamming_count = self.button_spamming_count + 1 if send_button == Buttons.RES_ACCEL else self.button_spamming_count - 1
return send_button
else:
self.button_spamming_count = 0
return 0
from openpilot.common.filter_simple import MyMovingAverage
class HyundaiJerk:
def __init__(self):
self.params = Params()
self.jerk = 0.0
self.jerk_u = self.jerk_l = 0.0
self.cb_upper = self.cb_lower = 0.0
self.jerk_u_min = 0.5
self.carrot_cruise = 1
self.carrot_cruise_accel = 0.0
def check_carrot_cruise(self, CC, CS, hud_control, stopping, accel, a_target):
carrot_cruise_decel = self.params.get_float("CarrotCruiseDecel")
carrot_cruise_atc_decel = self.params.get_float("CarrotCruiseAtcDecel")
if carrot_cruise_atc_decel >= 0 and 0 < hud_control.atcDistance < 500:
carrot_cruise_decel = max(carrot_cruise_decel, carrot_cruise_atc_decel)
self.carrot_cruise = 0
if CS.out.carrotCruise > 0 and not CC.cruiseControl.override:
if CS.softHoldActive == 0 and not stopping:
if CS.out.vEgo > 10/3.6:
if carrot_cruise_decel < 0:
if (a_target > -0.1 or accel > -0.1):
self.carrot_cruise = 1
self.carrot_cruise_accel = 0.0
else:
self.carrot_cruise = 2
carrot_cruise = min(accel, -carrot_cruise_decel * 0.01)
self.carrot_cruise_accel = max(carrot_cruise, self.carrot_cruise_accel - 1.0 * DT_CTRL) # 점진적으로 줄임.
if self.carrot_cruise == 0:
self.carrot_cruise_accel = CS.out.aEgo
def make_jerk(self, CP, CS, accel, actuators, hud_control):
if actuators.longControlState == LongCtrlState.stopping:
self.jerk = self.jerk_u_min / 2 - CS.out.aEgo
else:
jerk = actuators.jerk if actuators.longControlState == LongCtrlState.pid else 0.0
#a_error = actuators.aTarget - CS.out.aEgo
self.jerk = jerk# + a_error
jerk_max_l = 5.0
jerk_max_u = jerk_max_l
if actuators.longControlState == LongCtrlState.off:
self.jerk_u = jerk_max_u
self.jerk_l = jerk_max_l
self.cb_upper = self.cb_lower = 0.0
else:
if CP.flags & HyundaiFlags.CANFD:
self.jerk_u = min(max(self.jerk_u_min, self.jerk * 2.0), jerk_max_u)
self.jerk_l = min(max(1.0, -self.jerk * 4.0), jerk_max_l)
self.cb_upper = self.cb_lower = 0.0
else:
self.jerk_u = min(max(self.jerk_u_min, self.jerk * 2.0), jerk_max_u)
self.jerk_l = min(max(1.0, -self.jerk * 2.0), jerk_max_l)
self.cb_upper = np.clip(0.9 + accel * 0.2, 0, 1.2)
self.cb_lower = np.clip(0.8 + accel * 0.2, 0, 1.2)

683
opendbc_repo/opendbc/car/hyundai/carstate.py Normal file
View File

@@ -0,0 +1,683 @@
from collections import deque
import copy
import math
import numpy as np
import ast
from opendbc.can import CANDefine, CANParser
from opendbc.car import Bus, create_button_events, structs, DT_CTRL
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.values import HyundaiFlags, CAR, DBC, Buttons, CarControllerParams, CAMERA_SCC_CAR, HyundaiExtFlags
from opendbc.car.interfaces import CarStateBase
from openpilot.common.params import Params
from datetime import datetime
from zoneinfo import ZoneInfo
ButtonType = structs.CarState.ButtonEvent.Type
PREV_BUTTON_SAMPLES = 8
CLUSTER_SAMPLE_RATE = 20 # frames
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise,
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel, Buttons.LFA_BUTTON: ButtonType.lfaButton}
GearShifter = structs.CarState.GearShifter
NUMERIC_TO_TZ = {
840: "America/New_York", # 미국 (US) → 동부 시간대
124: "America/Toronto", # 캐나다 (CA) → 동부 시간대
250: "Europe/Paris", # 프랑스 (FR)
276: "Europe/Berlin", # 독일 (DE)
826: "Europe/London", # 영국 (GB)
392: "Asia/Tokyo", # 일본 (JP)
156: "Asia/Shanghai", # 중국 (CN)
410: "Asia/Seoul", # 한국 (KR)
36: "Australia/Sydney", # 호주 (AU)
356: "Asia/Kolkata", # 인도 (IN)
}
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.cruise_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.main_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.gear_msg_canfd = "GEAR" if CP.extFlags & HyundaiExtFlags.CANFD_GEARS_69 else \
"ACCELERATOR" if CP.flags & HyundaiFlags.EV else \
"GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
"GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_SHIFTER"
if CP.flags & HyundaiFlags.CANFD:
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
elif CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
self.shifter_values = can_define.dv["ELECT_GEAR"]["Elect_Gear_Shifter"]
elif self.CP.flags & HyundaiFlags.CLUSTER_GEARS:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.flags & HyundaiFlags.TCU_GEARS:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
elif CP.flags & HyundaiFlags.FCEV:
self.shifter_values = can_define.dv["EMS20"]["HYDROGEN_GEAR_SHIFTER"]
else:
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.accelerator_msg_canfd = "ACCELERATOR" if CP.flags & HyundaiFlags.EV else \
"ACCELERATOR_ALT" if CP.flags & HyundaiFlags.HYBRID else \
"ACCELERATOR_BRAKE_ALT"
self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \
"CRUISE_BUTTONS"
self.is_metric = False
self.buttons_counter = 0
self.cruise_info = {}
self.lfa_info = {}
self.lfa_alt_info = {}
self.lfahda_cluster_info = None
self.adrv_info_161 = None
self.adrv_info_200 = None
self.adrv_info_1ea = None
self.adrv_info_160 = None
self.adrv_info_162 = None
self.hda_info_4a3 = None
self.new_msg_4b4 = None
self.tcs_info_373 = None
self.mdps_info = {}
self.steer_touch_info = {}
self.cruise_buttons_msg = None
self.msg_0x362 = None
self.msg_0x2a4 = None
# On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
self.cluster_speed = 0
self.cluster_speed_counter = CLUSTER_SAMPLE_RATE
self.params = CarControllerParams(CP)
self.main_enabled = True if Params().get_int("AutoEngage") == 2 else False
self.gear_shifter = GearShifter.drive # Gear_init for Nexo ?? unknown 21.02.23.LSW
self.totalDistance = 0.0
self.speedLimitDistance = 0
self.pcmCruiseGap = 0
self.cruise_buttons_alt = True if self.CP.carFingerprint in (CAR.HYUNDAI_CASPER, CAR.HYUNDAI_CASPER_EV) else False
self.MainMode_ACC = False
self.ACCMode = 0
self.LFA_ICON = 0
self.paddle_button_prev = 0
self.lf_distance = 0
self.rf_distance = 0
self.lr_distance = 0
self.rr_distance = 0
#self.lf_lateral = 0
#self.rf_lateral = 0
fingerprints_str = Params().get("FingerPrints", encoding='utf-8')
fingerprints = ast.literal_eval(fingerprints_str)
#print("fingerprints =", fingerprints)
ecu_disabled = False
if self.CP.openpilotLongitudinalControl and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC):
ecu_disabled = True
if ecu_disabled:
self.SCC11 = self.SCC12 = self.SCC13 = self.SCC14 = self.FCA11 = False
else:
bus_cruise = 2 if self.CP.flags & HyundaiFlags.CAMERA_SCC else 0
self.SCC11 = True if 1056 in fingerprints[bus_cruise] else False
self.SCC12 = True if 1057 in fingerprints[bus_cruise] else False
self.SCC13 = True if 1290 in fingerprints[bus_cruise] else False
self.SCC14 = True if 905 in fingerprints[bus_cruise] else False
self.FCA11 = False
self.FCA11_bus = Bus.cam
self.HAS_LFA_BUTTON = True if 913 in fingerprints[0] else False
self.CRUISE_BUTTON_ALT = True if 1007 in fingerprints[0] else False
cam_bus = CanBus(CP).CAM
pt_bus = CanBus(CP).ECAN
alt_bus = CanBus(CP).ACAN
self.CCNC_0x161 = True if 0x161 in fingerprints[cam_bus] else False
self.CCNC_0x162 = True if 0x162 in fingerprints[cam_bus] else False
self.ADRV_0x200 = True if 0x200 in fingerprints[cam_bus] else False
self.ADRV_0x1ea = True if 0x1ea in fingerprints[cam_bus] else False
self.ADRV_0x160 = True if 0x160 in fingerprints[cam_bus] else False
self.LFAHDA_CLUSTER = True if 480 in fingerprints[cam_bus] else False
self.HDA_INFO_4A3 = True if 0x4a3 in fingerprints[pt_bus] else False
self.NEW_MSG_4B4 = True if 0x4b4 in fingerprints[pt_bus] else False
self.GEAR = True if 69 in fingerprints[pt_bus] else False
self.GEAR_ALT = True if 64 in fingerprints[pt_bus] else False
self.CAM_0x362 = True if 0x362 in fingerprints[alt_bus] else False
self.CAM_0x2a4 = True if 0x2a4 in fingerprints[alt_bus] else False
self.STEER_TOUCH_2AF = True if 0x2af in fingerprints[pt_bus] else False
self.TPMS = True if 0x3a0 in fingerprints[pt_bus] else False
self.LOCAL_TIME = True if 1264 in fingerprints[pt_bus] else False
self.cp_bsm = None
self.time_zone = "UTC"
self.controls_ready_count = 0
def update(self, can_parsers) -> structs.CarState:
if self.controls_ready_count <= 200:
if Params().get_bool("ControlsReady"):
self.controls_ready_count += 1
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
cp_alt = can_parsers[Bus.alt] if Bus.alt in can_parsers else None
if self.controls_ready_count == 50:
cp.controls_ready = cp_cam.controls_ready = True
if cp_alt is not None:
cp_alt.controls_ready = True
elif self.controls_ready_count == 100:
print("cp_cam.seen_addresses =", cp_cam.seen_addresses)
print("cp.seen_addresses =", cp.seen_addresses)
if 909 in cp_cam.seen_addresses:
self.FCA11 = True
self.FCA11_bus = Bus.cam
elif 909 in cp.seen_addresses:
self.FCA11 = True
self.FCA11_bus = Bus.pt
if cp_alt is not None:
print("cp_alt.seen_addresses =", cp_alt.seen_addresses)
if self.CP.flags & HyundaiFlags.CANFD:
return self.update_canfd(can_parsers)
ret = structs.CarState()
cp_cruise = cp_cam if self.CP.flags & HyundaiFlags.CAMERA_SCC else cp
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
self.cluster_speed_counter += 1
if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
self.cluster_speed_counter = 0
# Mimic how dash converts to imperial.
# Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric
# TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this
if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,):
self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
#ret.vEgoCluster = self.cluster_speed * speed_conv
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
# cruise state
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.available = self.main_enabled #cp.vl["TCS13"]["ACCEnable"] == 0
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
ret.cruiseState.nonAdaptive = False
elif not self.CP.flags & HyundaiFlags.CC_ONLY_CAR:
self.main_enabled = ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 2. # Shows 'Cruise Control' on dash
ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
ret.pcmCruiseGap = cp_cruise.vl["SCC11"]["TauGapSet"]
# TODO: Find brake pressure
ret.brake = 0
if not self.CP.flags & HyundaiFlags.CC_ONLY_CAR:
ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2 # 2 includes regen braking by user on HEV/EV
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
ret.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1
ret.espActive = cp.vl["TCS11"]["ABS_ACT"] == 1
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
ret.brakeLights = bool(cp.vl["TCS13"]["BrakeLight"] or ret.brakePressed)
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV | HyundaiFlags.FCEV):
if self.CP.flags & HyundaiFlags.FCEV:
ret.gas = cp.vl["FCEV_ACCELERATOR"]["ACCELERATOR_PEDAL"] / 254.
elif self.CP.flags & HyundaiFlags.HYBRID:
ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
else:
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
ret.gearStep = cp.vl["ELECT_GEAR"]["Elect_Gear_Step"]
elif self.CP.flags & HyundaiFlags.FCEV:
gear = cp.vl["EMS20"]["HYDROGEN_GEAR_SHIFTER"]
elif self.CP.flags & HyundaiFlags.CLUSTER_GEARS:
gear = cp.vl["CLU15"]["CF_Clu_Gear"]
elif self.CP.flags & HyundaiFlags.TCU_GEARS:
gear = cp.vl["TCU12"]["CUR_GR"]
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
ret.gearStep = cp.vl["LVR11"]["CF_Lvr_GearInf"]
if not self.CP.carFingerprint in (CAR.HYUNDAI_NEXO):
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
else:
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
gear_disp = cp.vl["ELECT_GEAR"]
gear_shifter = GearShifter.unknown
if gear == 1546: # Thank you for Neokii # fix PolorBear 22.06.05
gear_shifter = GearShifter.drive
elif gear == 2314:
gear_shifter = GearShifter.neutral
elif gear == 2569:
gear_shifter = GearShifter.park
elif gear == 2566:
gear_shifter = GearShifter.reverse
if gear_shifter != GearShifter.unknown and self.gear_shifter != gear_shifter:
self.gear_shifter = gear_shifter
ret.gearShifter = self.gear_shifter
if not self.CP.flags & HyundaiFlags.CC_ONLY_CAR and (not self.CP.openpilotLongitudinalControl or self.CP.flags & HyundaiFlags.CAMERA_SCC):
aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12"
aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
scc_warning = cp_cruise.vl["SCC12"]["TakeOverReq"] == 1 # sometimes only SCC system shows an FCW
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11 and CLU11
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
prev_cruise_buttons = self.cruise_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
#carrot {{
#if self.CRUISE_BUTTON_ALT and cp.vl["CRUISE_BUTTON_ALT"]["SET_ME_1"] == 1:
# self.cruise_buttons_alt = True
cruise_button = [Buttons.NONE]
if self.cruise_buttons_alt:
lfa_button = cp.vl["CRUISE_BUTTON_LFA"]["CruiseSwLfa"]
cruise_button = [Buttons.LFA_BUTTON] if lfa_button > 0 else [cp.vl["CRUISE_BUTTON_ALT"]["CruiseSwState"]]
elif self.HAS_LFA_BUTTON and cp.vl["BCM_PO_11"]["LFA_Pressed"] == 1: # for K5
cruise_button = [Buttons.LFA_BUTTON]
else:
cruise_button = cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]
self.cruise_buttons.extend(cruise_button)
# }} carrot
prev_main_buttons = self.main_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
if self.cruise_buttons_alt:
self.main_buttons.extend(cp.vl_all["CRUISE_BUTTON_ALT"]["CruiseSwMain"])
else:
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
self.mdps12 = copy.copy(cp.vl["MDPS12"])
ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})]
if not self.CP.flags & HyundaiFlags.CC_ONLY_CAR:
tpms_unit = cp.vl["TPMS11"]["UNIT"] * 0.725 if int(cp.vl["TPMS11"]["UNIT"]) > 0 else 1.
ret.tpms.fl = tpms_unit * cp.vl["TPMS11"]["PRESSURE_FL"]
ret.tpms.fr = tpms_unit * cp.vl["TPMS11"]["PRESSURE_FR"]
ret.tpms.rl = tpms_unit * cp.vl["TPMS11"]["PRESSURE_RL"]
ret.tpms.rr = tpms_unit * cp.vl["TPMS11"]["PRESSURE_RR"]
self.scc11 = cp_cruise.vl["SCC11"] if self.SCC11 else None
self.scc12 = cp_cruise.vl["SCC12"] if self.SCC12 else None
self.scc13 = cp_cruise.vl["SCC13"] if self.SCC13 else None
self.scc14 = cp_cruise.vl["SCC14"] if self.SCC14 else None
self.fca11 = can_parsers[self.FCA11_bus].vl["FCA11"] if self.FCA11 else None
cluSpeed = cp.vl["CLU11"]["CF_Clu_Vanz"]
decimal = cp.vl["CLU11"]["CF_Clu_VanzDecimal"]
if 0. < decimal < 0.5:
cluSpeed += decimal
ret.vEgoCluster = cluSpeed * speed_conv
vEgoClu, aEgoClu = self.update_clu_speed_kf(ret.vEgoCluster)
ret.vCluRatio = (ret.vEgo / vEgoClu) if (vEgoClu > 3. and ret.vEgo > 3.) else 1.0
if self.CP.extFlags & HyundaiExtFlags.NAVI_CLUSTER.value:
speedLimit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
speedLimitCam = cp.vl["Navi_HU"]["SpeedLim_Nav_Cam"]
ret.speedLimit = speedLimit if speedLimit < 255 and speedLimitCam == 1 else 0
speed_limit_cam = speedLimitCam == 1
else:
ret.speedLimit = 0
ret.speedLimitDistance = 0
speed_limit_cam = False
self.update_speed_limit(ret, speed_limit_cam)
if prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
return ret
def update_speed_limit(self, ret, speed_limit_cam):
self.totalDistance += ret.vEgo * DT_CTRL
if ret.speedLimit > 0 and not ret.gasPressed and speed_limit_cam:
if self.speedLimitDistance <= self.totalDistance:
self.speedLimitDistance = self.totalDistance + ret.speedLimit * 6
self.speedLimitDistance = max(self.totalDistance + 1, self.speedLimitDistance)
else:
self.speedLimitDistance = self.totalDistance
ret.speedLimitDistance = self.speedLimitDistance - self.totalDistance
def update_canfd(self, can_parsers) -> structs.CarState:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
cp_alt = can_parsers[Bus.alt] if Bus.alt in can_parsers else None
ret = structs.CarState()
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
if self.CP.flags & (HyundaiFlags.EV | HyundaiFlags.HYBRID):
offset = 255. if self.CP.flags & HyundaiFlags.EV else 1023.
ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset
ret.gasPressed = ret.gas > 1e-5
else:
ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"])
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
#print(cp.vl["TCS"], cp.vl_all["TCS"]["DriverBraking"][-10:])
ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR"] == 1
ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT"] == 0
gear = cp.vl[self.gear_msg_canfd]["GEAR"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
if self.TPMS:
tpms_unit = cp.vl["TPMS"]["UNIT"] * 0.725 if int(cp.vl["TPMS"]["UNIT"]) > 0 else 1.
ret.tpms.fl = tpms_unit * cp.vl["TPMS"]["PRESSURE_FL"]
ret.tpms.fr = tpms_unit * cp.vl["TPMS"]["PRESSURE_FR"]
ret.tpms.rl = tpms_unit * cp.vl["TPMS"]["PRESSURE_RL"]
ret.tpms.rr = tpms_unit * cp.vl["TPMS"]["PRESSURE_RR"]
# TODO: figure out positions
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
ret.brakeLights = ret.brakePressed or cp.vl["TCS"]["BrakeLight"] == 1
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
# steering angle deg값이 이상함. mdps값이 더 신뢰가 가는듯.. torque steering 차량도 확인해야함.
#ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
if self.CP.flags & HyundaiFlags.ANGLE_CONTROL:
ret.steeringAngleDeg = cp.vl["MDPS"]["STEERING_ANGLE_2"] * -1
else:
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 or cp.vl["MDPS"]["LFA2_FAULT"] != 0
#ret.steerFaultTemporary = False
self.mdps_info = copy.copy(cp.vl["MDPS"])
if self.STEER_TOUCH_2AF:
self.steer_touch_info = cp.vl["STEER_TOUCH_2AF"]
blinkers_info = cp.vl["BLINKERS"]
left_blinker_lamp = blinkers_info["LEFT_LAMP"] or blinkers_info["LEFT_LAMP_ALT"]
right_blinker_lamp = blinkers_info["RIGHT_LAMP"] or blinkers_info["RIGHT_LAMP_ALT"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, left_blinker_lamp, right_blinker_lamp)
if self.CP.enableBsm:
if self.cp_bsm is None:
if 442 in cp.seen_addresses:
self.cp_bsm = cp
print("######## BSM in ECAN")
elif 442 in cp_cam.seen_addresses:
self.cp_bsm = cp_cam
print("######## BSM in CAM")
else:
bsm_info = self.cp_bsm.vl["BLINDSPOTS_REAR_CORNERS"]
ret.leftBlindspot = (bsm_info["FL_INDICATOR"] + bsm_info["INDICATOR_LEFT_TWO"] + bsm_info["INDICATOR_LEFT_FOUR"]) > 0
ret.rightBlindspot = (bsm_info["FR_INDICATOR"] + bsm_info["INDICATOR_RIGHT_TWO"] + bsm_info["INDICATOR_RIGHT_FOUR"]) > 0
# cruise state
if cp.vl[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"] in [Buttons.RES_ACCEL, Buttons.SET_DECEL] and self.CP.openpilotLongitudinalControl:
self.main_enabled = True
# CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
ret.cruiseState.available = self.main_enabled #cp.vl["TCS"]["ACCEnable"] == 0
if self.CP.flags & HyundaiFlags.CAMERA_SCC.value:
self.MainMode_ACC = cp_cam.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
self.ACCMode = cp_cam.vl["SCC_CONTROL"]["ACCMode"]
self.LFA_ICON = cp_cam.vl["LFAHDA_CLUSTER"]["HDA_LFA_SymSta"]
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
if self.MainMode_ACC:
self.main_enabled = True
else:
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
if cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1: # carrot
ret.cruiseState.available = self.main_enabled = True
ret.pcmCruiseGap = int(np.clip(cp_cruise_info.vl["SCC_CONTROL"]["DISTANCE_SETTING"], 1, 4))
ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["InfoDisplay"] >= 4
ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
ret.brakeHoldActive = cp.vl["ESP_STATUS"]["AUTO_HOLD"] == 1 and cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] not in (1, 2)
speed_limit_cam = False
if self.CP.flags & HyundaiFlags.CAMERA_SCC.value:
self.cruise_info = copy.copy(cp_cam.vl["SCC_CONTROL"])
self.lfa_info = copy.copy(cp_cam.vl["LFA"])
if self.CP.flags & HyundaiFlags.ANGLE_CONTROL.value:
self.lfa_alt_info = copy.copy(cp_cam.vl["LFA_ALT"])
if self.LFAHDA_CLUSTER:
self.lfahda_cluster_info = cp_cam.vl["LFAHDA_CLUSTER"]
corner = False
self.adrv_info_161 = cp_cam.vl["ADRV_0x161"] if self.CCNC_0x161 else None
self.adrv_info_162 = cp_cam.vl["CCNC_0x162"] if self.CCNC_0x162 else None
if self.adrv_info_161 is not None:
ret.leftLongDist = self.lf_distance = self.adrv_info_162["LF_DETECT_DISTANCE"]
ret.rightLongDist = self.rf_distance = self.adrv_info_162["RF_DETECT_DISTANCE"]
self.lr_distance = self.adrv_info_162["LR_DETECT_DISTANCE"]
self.rr_distance = self.adrv_info_162["RR_DETECT_DISTANCE"]
ret.leftLatDist = self.adrv_info_162["LF_DETECT_LATERAL"]
ret.rightLatDist = self.adrv_info_162["RF_DETECT_LATERAL"]
corner = True
self.adrv_info_200 = cp_cam.vl["ADRV_0x200"] if self.ADRV_0x200 else None
self.adrv_info_1ea = cp_cam.vl["ADRV_0x1ea"] if self.ADRV_0x1ea else None
if self.adrv_info_1ea is not None:
if not corner:
ret.leftLongDist = self.adrv_info_1ea["LF_DETECT_DISTANCE"]
ret.rightLongDist = self.adrv_info_1ea["RF_DETECT_DISTANCE"]
ret.leftLatDist = self.adrv_info_1ea["LF_DETECT_LATERAL"]
ret.rightLatDist = self.adrv_info_1ea["RF_DETECT_LATERAL"]
self.adrv_info_160 = cp_cam.vl["ADRV_0x160"] if self.ADRV_0x160 else None
self.hda_info_4a3 = cp.vl["HDA_INFO_4A3"] if self.HDA_INFO_4A3 else None
if self.hda_info_4a3 is not None:
speedLimit = self.hda_info_4a3["SPEED_LIMIT"]
if not self.is_metric:
speedLimit *= CV.MPH_TO_KPH
ret.speedLimit = speedLimit if speedLimit < 255 else 0
if int(self.hda_info_4a3["MapSource"]) == 2:
speed_limit_cam = True
if self.time_zone == "UTC":
country_code = int(self.hda_info_4a3["CountryCode"])
self.time_zone = ZoneInfo(NUMERIC_TO_TZ.get(country_code, "UTC"))
self.new_msg_4b4 = cp.vl["NEW_MSG_4B4"] if self.NEW_MSG_4B4 else None
self.tcs_info_373 = cp.vl["TCS"]
ret.gearStep = cp.vl["GEAR"]["GEAR_STEP"] if self.GEAR else 0
if 1 <= ret.gearStep <= 8 and ret.gearShifter == GearShifter.unknown:
ret.gearShifter = GearShifter.drive
ret.gearStep = cp.vl["GEAR_ALT"]["GEAR_STEP"] if self.GEAR_ALT else ret.gearStep
if cp_alt and self.CP.flags & HyundaiFlags.CAMERA_SCC:
lane_info = None
lane_info = cp_alt.vl["CAM_0x362"] if self.CAM_0x362 else None
lane_info = cp_alt.vl["CAM_0x2a4"] if self.CAM_0x2a4 else lane_info
if lane_info is not None:
left_lane_prob = lane_info["LEFT_LANE_PROB"]
right_lane_prob = lane_info["RIGHT_LANE_PROB"]
left_lane_type = lane_info["LEFT_LANE_TYPE"] # 0: dashed, 1: solid, 2: undecided, 3: road edge, 4: DLM Inner Solid, 5: DLM InnerDashed, 6:DLM Inner Undecided, 7: Botts Dots, 8: Barrier
right_lane_type = lane_info["RIGHT_LANE_TYPE"]
left_lane_color = lane_info["LEFT_LANE_COLOR"]
right_lane_color = lane_info["RIGHT_LANE_COLOR"]
left_lane_info = left_lane_color * 10 + left_lane_type
right_lane_info = right_lane_color * 10 + right_lane_type
ret.leftLaneLine = left_lane_info
ret.rightLaneLine = right_lane_info
# Manual Speed Limit Assist is a feature that replaces non-adaptive cruise control on EV CAN FD platforms.
# It limits the vehicle speed, overridable by pressing the accelerator past a certain point.
# The car will brake, but does not respect positive acceleration commands in this mode
# TODO: find this message on ICE & HYBRID cars + cruise control signals (if exists)
if self.CP.flags & HyundaiFlags.EV:
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
if self.LOCAL_TIME and self.time_zone != "UTC":
lt = cp.vl["LOCAL_TIME"]
y, m, d, H, M, S = int(lt["YEAR"]) + 2000, int(lt["MONTH"]), int(lt["DATE"]), int(lt["HOURS"]), int(lt["MINUTES"]), int(lt["SECONDS"])
try:
dt_local = datetime(y, m, d, H, M, S, tzinfo=self.time_zone)
ret.datetime = int(dt_local.timestamp() * 1000)
except:
#print(f"Error parsing local time: {y}-{m}-{d} {H}:{M}:{S} in {self.time_zone}")
pass
prev_cruise_buttons = self.cruise_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
#carrot {{
if cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]:
cruise_button = [Buttons.LFA_BUTTON]
else:
cruise_button = cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]
self.cruise_buttons.extend(cruise_button)
# }} carrot
if self.cruise_btns_msg_canfd in cp.vl:
self.cruise_buttons_msg = copy.copy(cp.vl[self.cruise_btns_msg_canfd])
"""
if self.cruise_btns_msg_canfd in cp.vl: #carrot
if not cp.vl[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]:
pass
#print("empty cruise btns...")
else:
self.cruise_buttons_msg = copy.copy(cp.vl[self.cruise_btns_msg_canfd])
"""
prev_main_buttons = self.main_buttons[-1]
#self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
if self.main_buttons[-1] != prev_main_buttons and not self.main_buttons[-1]: # and self.CP.openpilotLongitudinalControl: #carrot
self.main_enabled = not self.main_enabled
print("main_enabled = {}".format(self.main_enabled))
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if not (self.CP.flags & HyundaiFlags.CAMERA_SCC):
if self.msg_0x362 is not None or 0x362 in cp_cam.seen_addresses:
self.msg_0x362 = cp_cam.vl["CAM_0x362"]
elif self.msg_0x2a4 is not None or 0x2a4 in cp_cam.seen_addresses:
self.msg_0x2a4 = cp_cam.vl["CAM_0x2a4"]
speed_conv = CV.KPH_TO_MS # if self.is_metric else CV.MPH_TO_MS
cluSpeed = cp.vl["CRUISE_BUTTONS_ALT"]["CLU_SPEED"]
ret.vEgoCluster = cluSpeed * speed_conv # MPH단위에서도 KPH로 나오는듯..
vEgoClu, aEgoClu = self.update_clu_speed_kf(ret.vEgoCluster)
ret.vCluRatio = (ret.vEgo / vEgoClu) if (vEgoClu > 3. and ret.vEgo > 3.) else 1.0
self.update_speed_limit(ret, speed_limit_cam)
paddle_button = self.paddle_button_prev
if self.cruise_btns_msg_canfd == "CRUISE_BUTTONS":
paddle_button = 1 if cp.vl["CRUISE_BUTTONS"]["LEFT_PADDLE"] == 1 else 2 if cp.vl["CRUISE_BUTTONS"]["RIGHT_PADDLE"] == 1 else 0
elif self.gear_msg_canfd == "GEAR":
paddle_button = 1 if cp.vl["GEAR"]["LEFT_PADDLE"] == 1 else 2 if cp.vl["GEAR"]["RIGHT_PADDLE"] == 1 else 0
ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(paddle_button, self.paddle_button_prev, {1: ButtonType.paddleLeft, 2: ButtonType.paddleRight}),
*create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})]
self.paddle_button_prev = paddle_button
return ret
def get_can_parsers_canfd(self, CP):
msgs = []
if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
# TODO: this can be removed once we add dynamic support to vl_all
msgs += [
("CRUISE_BUTTONS", 50)
]
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CanBus(CP).ECAN),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).CAM),
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).ACAN),
}
def get_can_parsers(self, CP):
if CP.flags & HyundaiFlags.CANFD:
return self.get_can_parsers_canfd(CP)
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
}

1322
opendbc_repo/opendbc/car/hyundai/fingerprints.py Normal file
View File

File diff suppressed because it is too large Load Diff

383
opendbc_repo/opendbc/car/hyundai/hyundaican.py Normal file
View File

@@ -0,0 +1,383 @@
import copy
import crcmod
from opendbc.car.hyundai.values import CAR, HyundaiFlags
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
def create_lkas11(packer, frame, CP, apply_torque, steer_req,
torque_fault, lkas11, sys_warning, sys_state, enabled,
left_lane, right_lane,
left_lane_depart, right_lane_depart, is_ldws_car):
values = {s: lkas11[s] for s in [
"CF_Lkas_LdwsActivemode",
"CF_Lkas_LdwsSysState",
"CF_Lkas_SysWarning",
"CF_Lkas_LdwsLHWarning",
"CF_Lkas_LdwsRHWarning",
"CF_Lkas_HbaLamp",
"CF_Lkas_FcwBasReq",
"CF_Lkas_HbaSysState",
"CF_Lkas_FcwOpt",
"CF_Lkas_HbaOpt",
"CF_Lkas_FcwSysState",
"CF_Lkas_FcwCollisionWarning",
"CF_Lkas_FusionState",
"CF_Lkas_FcwOpt_USM",
"CF_Lkas_LdwsOpt_USM",
]}
values["CF_Lkas_LdwsSysState"] = sys_state
values["CF_Lkas_SysWarning"] = 0 # 3 if sys_warning else 0
values["CF_Lkas_LdwsLHWarning"] = left_lane_depart
values["CF_Lkas_LdwsRHWarning"] = right_lane_depart
values["CR_Lkas_StrToqReq"] = apply_torque
values["CF_Lkas_ActToi"] = steer_req
values["CF_Lkas_ToiFlt"] = torque_fault # seems to allow actuation on CR_Lkas_StrToqReq
values["CF_Lkas_MsgCount"] = frame % 0x10
if CP.flags & HyundaiFlags.SEND_LFA.value or CP.carFingerprint in (CAR.HYUNDAI_SANTA_FE):
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2
# FcwOpt_USM 5 = Orange blinking car + lanes
# FcwOpt_USM 4 = Orange car + lanes
# FcwOpt_USM 3 = Green blinking car + lanes
# FcwOpt_USM 2 = Green car + lanes
# FcwOpt_USM 1 = White car + lanes
# FcwOpt_USM 0 = No car + lanes
values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1
# SysWarning 4 = keep hands on wheel
# SysWarning 5 = keep hands on wheel (red)
# SysWarning 6 = keep hands on wheel (red) + beep
# Note: the warning is hidden while the blinkers are on
values["CF_Lkas_SysWarning"] = 0 #4 if sys_warning else 0
# Likely cars lacking the ability to show individual lane lines in the dash
elif CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL):
# SysWarning 4 = keep hands on wheel + beep
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
# SysState 0 = no icons
# SysState 1-2 = white car + lanes
# SysState 3 = green car + lanes, green steering wheel
# SysState 4 = green car + lanes
values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1
values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition
# these have no effect
values["CF_Lkas_LdwsActivemode"] = 0
values["CF_Lkas_FcwOpt_USM"] = 0
elif CP.carFingerprint == CAR.HYUNDAI_GENESIS:
# This field is actually LdwsActivemode
# Genesis and Optima fault when forwarding while engaged
values["CF_Lkas_LdwsActivemode"] = 2
if is_ldws_car:
values["CF_Lkas_LdwsOpt_USM"] = 3
dat = packer.make_can_msg("LKAS11", 0, values)[1]
if CP.flags & HyundaiFlags.CHECKSUM_CRC8:
# CRC Checksum as seen on 2019 Hyundai Santa Fe
dat = dat[:6] + dat[7:8]
checksum = hyundai_checksum(dat)
elif CP.flags & HyundaiFlags.CHECKSUM_6B:
# Checksum of first 6 Bytes, as seen on 2018 Kia Sorento
checksum = sum(dat[:6]) % 256
else:
# Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger
checksum = (sum(dat[:6]) + dat[7]) % 256
values["CF_Lkas_Chksum"] = checksum
return packer.make_can_msg("LKAS11", 0, values)
def create_clu11(packer, frame, clu11, button, CP):
values = {s: clu11[s] for s in [
"CF_Clu_CruiseSwState",
"CF_Clu_CruiseSwMain",
"CF_Clu_SldMainSW",
"CF_Clu_ParityBit1",
"CF_Clu_VanzDecimal",
"CF_Clu_Vanz",
"CF_Clu_SPEED_UNIT",
"CF_Clu_DetentOut",
"CF_Clu_RheostatLevel",
"CF_Clu_CluInfo",
"CF_Clu_AmpInfo",
"CF_Clu_AliveCnt1",
]}
values["CF_Clu_CruiseSwState"] = button
values["CF_Clu_AliveCnt1"] = frame % 0x10
# send buttons to camera on camera-scc based cars
bus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC else 0
return packer.make_can_msg("CLU11", bus, values)
def create_lfahda_mfc(packer, CC, blinking_signal):
activeCarrot = CC.hudControl.activeCarrot
values = {
"LFA_Icon_State": 2 if CC.latActive else 1 if CC.enabled else 0,
#"HDA_Active": 1 if activeCarrot >= 2 else 0,
#"HDA_Icon_State": 2 if activeCarrot == 3 and blinking_signal else 2 if activeCarrot >= 2 else 0,
"HDA_Icon_State": 0 if activeCarrot == 3 and blinking_signal else 2 if activeCarrot >= 1 else 0,
"HDA_VSetReq": 0, #set_speed_in_units if activeCarrot >= 2 else 0,
"HDA_USM" : 2,
"HDA_Icon_Wheel" : 1 if CC.latActive else 0,
#"HDA_Chime" : 1 if CC.latActive else 0, # comment for K9 chime,
}
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands_scc(packer, enabled, accel, jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CS, soft_hold_mode):
from opendbc.car.hyundai.carcontroller import HyundaiJerk
cruise_available = CS.out.cruiseState.available
if CS.paddle_button_prev > 0:
cruise_available = False
soft_hold_active = CS.softHoldActive
soft_hold_info = soft_hold_active > 1 and enabled
#soft_hold_mode = 2 ## some cars can't enable while braking
long_enabled = enabled or (soft_hold_active > 0 and soft_hold_mode == 2)
stop_req = 1 if stopping or (soft_hold_active > 0 and soft_hold_mode == 2) else 0
d = hud_control.leadDistance
objGap = 0 if d == 0 else 2 if d < 25 else 3 if d < 40 else 4 if d < 70 else 5
objGap2 = 0 if objGap == 0 else 2 if hud_control.leadRelSpeed < -0.2 else 1
if long_enabled:
if jerk.carrot_cruise == 1:
long_enabled = False
accel = -0.5
elif jerk.carrot_cruise == 2:
accel = jerk.carrot_cruise_accel
if long_enabled:
scc12_acc_mode = 2 if long_override else 1
scc14_acc_mode = 2 if long_override else 1
if CS.out.brakeHoldActive:
scc12_acc_mode = 0
scc14_acc_mode = 4
elif CS.out.brakePressed:
scc12_acc_mode = 1
scc14_acc_mode = 1
else:
scc12_acc_mode = 0
scc14_acc_mode = 4
warning_front = False
commands = []
if CS.scc11 is not None:
values = copy.copy(CS.scc11)
values["MainMode_ACC"] = 1 if cruise_available else 0
values["TauGapSet"] = hud_control.leadDistanceBars
values["VSetDis"] = set_speed if enabled else 0
values["AliveCounterACC"] = idx % 0x10
values["SCCInfoDisplay"] = 3 if warning_front else 4 if soft_hold_info else 0 if enabled else 0 #2: 크루즈 선택, 3: 전방상황주의, 4: 출발준비
values["ObjValid"] = 1 if hud_control.leadVisible else 0
values["ACC_ObjStatus"] = 1 if hud_control.leadVisible else 0
values["ACC_ObjLatPos"] = 0
values["ACC_ObjRelSpd"] = hud_control.leadRelSpeed
values["ACC_ObjDist"] = int(hud_control.leadDistance)
values["DriverAlertDisplay"] = 0
commands.append(packer.make_can_msg("SCC11", 0, values))
if CS.scc12 is not None:
values = copy.copy(CS.scc12)
values["ACCMode"] = scc12_acc_mode #2 if enabled and long_override else 1 if long_enabled else 0
values["StopReq"] = stop_req
values["aReqRaw"] = accel
values["aReqValue"] = accel
values["ACCFailInfo"] = 0
#values["DESIRED_DIST"] = CS.out.vEgo * 1.0 + 4.0 # TF: 1.0 + STOPDISTANCE 4.0 m로 가정함.
values["CR_VSM_ChkSum"] = 0
values["CR_VSM_Alive"] = idx % 0xF
scc12_dat = packer.make_can_msg("SCC12", 0, values)[1]
values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
commands.append(packer.make_can_msg("SCC12", 0, values))
if CS.scc14 is not None:
values = copy.copy(CS.scc14)
values["ComfortBandUpper"] = jerk.cb_upper
values["ComfortBandLower"] = jerk.cb_lower
values["JerkUpperLimit"] = jerk.jerk_u
values["JerkLowerLimit"] = jerk.jerk_l if long_enabled else 0 # for KONA test
values["ACCMode"] = scc14_acc_mode #2 if enabled and long_override else 1 if long_enabled else 4 # stock will always be 4 instead of 0 after first disengage
values["ObjGap"] = objGap #2 if hud_control.leadVisible else 0 # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
values["ObjDistStat"] = objGap2
commands.append(packer.make_can_msg("SCC14", 0, values))
if CS.fca11 is not None and use_fca: # CASPER_EV의 경우 FCA11에서 fail이 간헐적 발생함.. 그냥막자.. 원인불명..
values = copy.copy(CS.fca11)
if values["FCA_Failinfo"] != 0:
values["FCA_Status"] = 2
values["FCA_Failinfo"] = 0
fca11_dat = packer.make_can_msg("FCA11", 0, values)[1]
values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, values))
# Only send FCA11 on cars where it exists on the bus
if False: #use_fca:
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
"CR_FCA_Alive": idx % 0xF,
"PAINT1_Status": 1,
"FCA_DrvSetStatus": 1,
"FCA_Status": 1, # AEB disabled
}
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[1]
fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
return commands
def create_acc_opt_copy(CS, packer):
values = copy.copy(CS.scc13)
if values["NEW_SIGNAL_1"] == 255:
values["NEW_SIGNAL_1"] = 218
values["NEW_SIGNAL_2"] = 0
return packer.make_can_msg("SCC13", 0, CS.scc13)
def create_acc_commands(packer, enabled, accel, jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP, CS, soft_hold_mode):
from opendbc.car.hyundai.carcontroller import HyundaiJerk
cruise_available = CS.out.cruiseState.available
soft_hold_active = CS.softHoldActive
soft_hold_info = soft_hold_active > 1 and enabled
#soft_hold_mode = 2 ## some cars can't enable while braking
long_enabled = enabled or (soft_hold_active > 0 and soft_hold_mode == 2)
stop_req = 1 if stopping or (soft_hold_active > 0 and soft_hold_mode == 2) else 0
d = hud_control.leadDistance
objGap = 0 if d == 0 else 2 if d < 25 else 3 if d < 40 else 4 if d < 70 else 5
objGap2 = 0 if objGap == 0 else 2 if hud_control.leadRelSpeed < -0.2 else 1
if long_enabled:
scc12_acc_mode = 2 if long_override else 1
scc14_acc_mode = 2 if long_override else 1
if CS.out.brakeHoldActive:
scc12_acc_mode = 0
scc14_acc_mode = 4
elif CS.out.brakePressed:
scc12_acc_mode = 1
scc14_acc_mode = 1
else:
scc12_acc_mode = 0
scc14_acc_mode = 4
warning_front = False
commands = []
scc11_values = {
"MainMode_ACC": 1 if cruise_available else 0,
"TauGapSet": hud_control.leadDistanceBars,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,
"SCCInfoDisplay": 3 if warning_front else 4 if soft_hold_info else 0 if enabled else 0,
"ObjValid": 1 if hud_control.leadVisible else 0, # close lead makes controls tighter
"ACC_ObjStatus": 1 if hud_control.leadVisible else 0, # close lead makes controls tighter
"ACC_ObjLatPos": 0,
"ACC_ObjRelSpd": hud_control.leadRelSpeed,
"ACC_ObjDist": int(hud_control.leadDistance), # close lead makes controls tighter
"DriverAlertDisplay": 0,
}
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
scc12_values = {
"ACCMode": scc12_acc_mode,
"StopReq": stop_req,
"aReqRaw": 0 if stop_req > 0 else accel,
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
#"DESIRED_DIST": CS.out.vEgo * 1.0 + 4.0,
"CR_VSM_Alive": idx % 0xF,
}
# show AEB disabled indicator on dash with SCC12 if not sending FCA messages.
# these signals also prevent a TCS fault on non-FCA cars with alpha longitudinal
if not use_fca:
scc12_values["CF_VSM_ConfMode"] = 1
scc12_values["AEB_Status"] = 1 # AEB disabled
scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[1]
scc12_values["CR_VSM_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in scc12_dat) % 0x10
commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
scc14_values = {
"ComfortBandUpper": jerk.cb_upper, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": jerk.cb_lower, # stock usually is 0 but sometimes uses higher values
"JerkUpperLimit": jerk.jerk_u, # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": jerk.jerk_l, # stock usually is 0.5 but sometimes uses higher values
"ACCMode": scc14_acc_mode, # if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
"ObjGap": objGap, #2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
"ObjDistStat": objGap2,
}
commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
# Only send FCA11 on cars where it exists on the bus
# On Camera SCC cars, FCA11 is not disabled, so we forward stock FCA11 back to the car forward hooks
if use_fca and not (CP.flags & HyundaiFlags.CAMERA_SCC):
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
"CR_FCA_Alive": idx % 0xF,
"PAINT1_Status": 1,
"FCA_DrvSetStatus": 1,
"FCA_Status": 1, # AEB disabled
}
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[1]
fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
return commands
def create_acc_opt(packer, CP):
commands = []
scc13_values = {
"SCCDrvModeRValue": 2,
"SCC_Equip": 1,
"Lead_Veh_Dep_Alert_USM": 2,
}
commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
# TODO: this needs to be detected and conditionally sent on unsupported long cars
# On Camera SCC cars, FCA12 is not disabled, so we forward stock FCA12 back to the car forward hooks
if not (CP.flags & HyundaiFlags.CAMERA_SCC):
fca12_values = {
"FCA_DrvSetState": 2,
"FCA_USM": 1, # AEB disabled
}
commands.append(packer.make_can_msg("FCA12", 0, fca12_values))
return commands
def create_frt_radar_opt(packer):
frt_radar11_values = {
"CF_FCA_Equip_Front_Radar": 1,
}
return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values)
def create_clu11_button(packer, frame, clu11, button, CP):
values = clu11
values["CF_Clu_CruiseSwState"] = button
#values["CF_Clu_AliveCnt1"] = frame % 0x10
values["CF_Clu_AliveCnt1"] = (values["CF_Clu_AliveCnt1"] + 1) % 0x10
# send buttons to camera on camera-scc based cars
bus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC else 0
return packer.make_can_msg("CLU11", bus, values)
def create_mdps12(packer, frame, mdps12):
values = mdps12
values["CF_Mdps_ToiActive"] = 0
values["CF_Mdps_ToiUnavail"] = 1
values["CF_Mdps_MsgCount2"] = frame % 0x100
values["CF_Mdps_Chksum2"] = 0
dat = packer.make_can_msg("MDPS12", 2, values)[1]
checksum = sum(dat) % 256
values["CF_Mdps_Chksum2"] = checksum
return packer.make_can_msg("MDPS12", 2, values)

729
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py Normal file
View File

@@ -0,0 +1,729 @@
import copy
import numpy as np
from opendbc.car import CanBusBase
from opendbc.car.crc import CRC16_XMODEM
from opendbc.car.hyundai.values import HyundaiFlags, HyundaiExtFlags
from openpilot.common.params import Params
from opendbc.car.common.conversions import Conversions as CV
def hyundai_crc8(data: bytes) -> int:
poly = 0x2F
crc = 0xFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x80:
crc = ((crc << 1) ^ poly) & 0xFF
else:
crc = (crc << 1) & 0xFF
return crc ^ 0xFF
class CanBus(CanBusBase):
def __init__(self, CP, fingerprint=None, lka_steering=None) -> None:
super().__init__(CP, fingerprint)
if lka_steering is None:
lka_steering = CP.flags & HyundaiFlags.CANFD_HDA2.value if CP is not None else False
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. LKA steering cars
# have a different harness than the LFA steering variants in order to split
# a different bus, since the steering is done by different ECUs.
self._a, self._e = 1, 0
if lka_steering and Params().get_int("HyundaiCameraSCC") == 0: #배선개조는 무조건 Bus0가 ECAN임.
self._a, self._e = 0, 1
self._a += self.offset
self._e += self.offset
self._cam = 2 + self.offset
@property
def ECAN(self):
return self._e
@property
def ACAN(self):
return self._a
@property
def CAM(self):
return self._cam
# CAN LIST (CAM) - 롱컨개조시... ADAS + CAM
# 160: ADRV_0x160
# 1da: ADRV_0x1da
# 1ea: ADRV_0x1ea
# 200: ADRV_0x200
# 345: ADRV_0x345
# 1fa: CLUSTER_SPEED_LIMIT
# 12a: LFA
# 1e0: LFAHDA_CLUSTER
# 11a:
# 1b5:
# 1a0: SCC_CONTROL
# CAN LIST (ACAN)
# 160: ADRV_0x160
# 51: ADRV_0x51
# 180: CAM_0x180
# ...
# 185: CAM_0x185
# 1b6: CAM_0x1b6
# ...
# 1b9: CAM_0x1b9
# 1fb: CAM_0x1fb
# 2a2 - 2a4
# 2bb - 2be
# LKAS
# 201 - 2a0
def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active, apply_steer, CS, apply_angle, max_torque, angle_control):
emergency_steering = False
if CS.adrv_info_161 is not None:
values = CS.adrv_info_161
emergency_steering = values["ALERTS_1"] in [11, 12, 13, 14, 15, 21, 22, 23, 24, 25, 26]
ret = []
values = copy.copy(CS.mdps_info)
if angle_control:
if CS.lfa_alt_info is not None:
values["LFA2_ACTIVE"] = CS.lfa_alt_info["LKAS_ANGLE_ACTIVE"]
else:
if CS.lfa_info is not None:
values["LKA_ACTIVE"] = 1 if CS.lfa_info["STEER_REQ"] == 1 else 0
if frame % 1000 < 40:
values["STEERING_COL_TORQUE"] += 220
ret.append(packer.make_can_msg("MDPS", CAN.CAM, values))
if frame % 10 == 0:
if CS.steer_touch_info is not None:
values = copy.copy(CS.steer_touch_info)
if frame % 1000 < 40:
values["TOUCH_DETECT"] = 3
values["TOUCH1"] = 50
values["TOUCH2"] = 50
values["CHECKSUM_"] = 0
dat = packer.make_can_msg("STEER_TOUCH_2AF", 0, values)[1]
values["CHECKSUM_"] = hyundai_crc8(dat[1:8])
ret.append(packer.make_can_msg("STEER_TOUCH_2AF", CAN.CAM, values))
if angle_control:
if emergency_steering:
values = copy.copy(CS.lfa_alt_info)
else:
values = {} #CS.lfa_alt_info
values["LKAS_ANGLE_ACTIVE"] = 2 if CC.latActive else 1
values["LKAS_ANGLE_CMD"] = -apply_angle
values["LKAS_ANGLE_MAX_TORQUE"] = max_torque if CC.latActive else 0
ret.append(packer.make_can_msg("LFA_ALT", CAN.ECAN, values))
values = copy.copy(CS.lfa_info)
if not emergency_steering:
values["LKA_MODE"] = 0
values["LKA_ICON"] = 2 if CC.latActive else 1
values["TORQUE_REQUEST"] = -1024 # apply_steer,
values["VALUE63"] = 0 # LKA_ASSIST
values["STEER_REQ"] = 0 # 1 if lat_active else 0,
values["HAS_LANE_SAFETY"] = 0 # hide LKAS settings
values["LKA_ACTIVE"] = 3 if CC.latActive else 0 # this changes sometimes, 3 seems to indicate engaged
values["VALUE64"] = 0 #STEER_MODE, NEW_SIGNAL_2
values["LKAS_ANGLE_CMD"] = -25.6 #-apply_angle,
values["LKAS_ANGLE_ACTIVE"] = 0 #2 if lat_active else 1,
values["LKAS_ANGLE_MAX_TORQUE"] = 0 #max_torque if lat_active else 0,
values["NEW_SIGNAL_1"] = 10
else:
values = {}
values["LKA_MODE"] = 2
values["LKA_ICON"] = 2 if lat_active else 1
values["TORQUE_REQUEST"] = apply_steer
values["STEER_REQ"] = 1 if lat_active else 0
values["VALUE64"] = 0 # STEER_MODE, NEW_SIGNAL_2
values["HAS_LANE_SAFETY"] = 0
values["LKA_ACTIVE"] = 0 # NEW_SIGNAL_1
values["DampingGain"] = 0 if lat_active else 100
#values["VALUE63"] = 0
#values["VALUE82_SET256"] = 0
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
return ret
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer, apply_angle, max_torque, angle_control):
ret = []
if angle_control:
values = {
"LKA_MODE": 0,
"LKA_ICON": 2 if enabled else 1,
"TORQUE_REQUEST": 0, # apply_steer,
"VALUE63": 0, # LKA_ASSIST
"STEER_REQ": 0, # 1 if lat_active else 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
"LKA_ACTIVE": 3 if lat_active else 0, # this changes sometimes, 3 seems to indicate engaged
"VALUE64": 0, #STEER_MODE, NEW_SIGNAL_2
"LKAS_ANGLE_CMD": -apply_angle,
"LKAS_ANGLE_ACTIVE": 2 if lat_active else 1,
"LKAS_ANGLE_MAX_TORQUE": max_torque if lat_active else 0,
# test for EV6PE
"NEW_SIGNAL_1": 10, #2,
"DampingGain": 9,
"VALUE231": 146,
"VALUE239": 1,
"VALUE247": 255,
"VALUE255": 255,
}
else:
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"TORQUE_REQUEST": apply_steer,
"DampingGain": 3 if enabled else 100,
"STEER_REQ": 1 if lat_active else 0,
#"STEER_MODE": 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
"VALUE63": 0,
"VALUE64": 0,
}
if CP.flags & HyundaiFlags.CANFD_HDA2:
lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "LKAS"
if CP.openpilotLongitudinalControl:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
if not (CP.flags & HyundaiFlags.CAMERA_SCC.value):
ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, values))
else:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
return ret
def create_suppress_lfa(packer, CAN, CS):
if CS.msg_0x362 is not None:
suppress_msg = "CAM_0x362"
lfa_block_msg = CS.msg_0x362
elif CS.msg_0x2a4 is not None:
suppress_msg = "CAM_0x2a4"
lfa_block_msg = CS.msg_0x2a4
else:
return []
#values = {f"BYTE{i}": lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7}
values = copy.copy(lfa_block_msg)
values["COUNTER"] = lfa_block_msg["COUNTER"]
values["SET_ME_0"] = 0
values["SET_ME_0_2"] = 0
values["LEFT_LANE_LINE"] = 0
values["RIGHT_LANE_LINE"] = 0
return [packer.make_can_msg(suppress_msg, CAN.ACAN, values)]
def create_buttons(packer, CP, CAN, cnt, btn):
values = {
"COUNTER": cnt,
"SET_ME_1": 1,
"CRUISE_BUTTONS": btn,
}
#bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
bus = CAN.ECAN
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
# TODO: why do we copy different values here?
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"NEW_SIGNAL_1",
"MainMode_ACC",
"ACCMode",
"ZEROS_9",
"CRUISE_STANDSTILL",
"ZEROS_5",
"DISTANCE_SETTING",
"VSetDis",
]}
else:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"ACCMode",
"VSetDis",
"CRUISE_STANDSTILL",
]}
values.update({
"ACCMode": 4,
"aReqRaw": 0.0,
"aReqValue": 0.0,
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CS, CAN, long_active, lat_active):
if CS.lfahda_cluster_info is not None:
values = {} #
values["HDA_CntrlModSta"] = 2 if long_active else 0
values["HDA_LFA_SymSta"] = 2 if lat_active else 0
#
else:
return []
return [packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)]
def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, hyundai_jerk, CS):
enabled = (enabled or CS.softHoldActive > 0) and CS.paddle_button_prev == 0
acc_mode = 0 if not enabled else (2 if gas_override else 1)
if hyundai_jerk.carrot_cruise == 1:
acc_mode = 4 if enabled else 0
enabled = False
accel = accel_last = 0.5
elif hyundai_jerk.carrot_cruise == 2:
accel = accel_last = hyundai_jerk.carrot_cruise_accel
jerk_u = hyundai_jerk.jerk_u
jerk_l = hyundai_jerk.jerk_l
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
a_val, a_raw = 0, 0
else:
a_raw = accel
a_val = np.clip(accel, accel_last - jn, accel_last + jn)
values = copy.copy(CS.cruise_info)
values["ACCMode"] = acc_mode
values["MainMode_ACC"] = 1
values["StopReq"] = 1 if stopping or CS.softHoldActive > 0 else 0 # 1: Stop control is required, 2: Not used, 3: Error Indicator
values["aReqValue"] = a_val
values["aReqRaw"] = a_raw
values["VSetDis"] = set_speed
#values["JerkLowerLimit"] = jerk if enabled else 1
#values["JerkUpperLimit"] = 3.0
values["JerkLowerLimit"] = jerk_l if enabled else 1
values["JerkUpperLimit"] = 2.0 if stopping or CS.softHoldActive else jerk_u
values["DISTANCE_SETTING"] = hud_control.leadDistanceBars # + 5
#values["DISTANCE_SETTING"] = hud_control.leadDistanceBars + 5
#values["ACC_ObjDist"] = 1
#values["ObjValid"] = 0
#values["OBJ_STATUS"] = 2
values["NSCCOper"] = 1 if enabled else 0 # 0: off, 1: Ready, 2: Act, 3: Error Indicator
values["NSCCOnOff"] = 2 # 0: Default, 1: Off, 2: On, 3: Invalid
#values["SET_ME_3"] = 0x3 # objRelsped와 충돌
#values["ACC_ObjLatPos"] = - hud_control.leadDPath
values["DriveMode"] = 0 # 0: Default, 1: Comfort Mode, 2:Normal mode, 3:Dynamic mode, reserved
hud_lead_info = 0
if hud_control.leadVisible:
hud_lead_info = 1 if values["ACC_ObjRelSpd"] > 0 else 2
values["HUD_LEAD_INFO"] = hud_lead_info #1: in-path object detected(uncontrollable), 2: controllable long, 3: controllable long & lat, ... reserved
values["DriverAlert"] = 0 # 1: SCC Disengaged, 2: No SCC Engage condition, 3: SCC Disenganed when the vehicle stops
values["TARGET_DISTANCE"] = CS.out.vEgo * 1.0 + 4.0
soft_hold_info = 1 if CS.softHoldActive > 1 and enabled else 0
# 이거안하면 정지중 뒤로 밀리는 현상 발생하는듯.. (신호정지중에 뒤로 밀리는 경험함.. 시험해봐야)
if values["InfoDisplay"] != 5: #5: Front Car Departure Notice
values["InfoDisplay"] = 4 if stopping and CS.out.aEgo > -0.3 else 0 # 1: SCC Mode, 2: Convention Cruise Mode, 3: Object disappered at low speed, 4: Available to resume acceleration control, 5: Front vehicle departure notice, 6: Reserved, 7: Invalid
values["TakeOverReq"] = 0 # 1: Takeover request, 2: Not used, 3: Error indicator , 이것이 켜지면 가속을 안하는듯함.
#values["NEW_SIGNAL_4"] = 9 if hud_control.leadVisible else 0
# AccelLimitBandUpper, Lower
values["SysFailState"] = 0 # 1: Performance degredation, 2: system temporairy unavailble, 3: SCC Service required , 눈이 묻어 레이더오류시... 2가 됨. 이때 가속을 안함...
values["AccelLimitBandUpper"] = 0.0 # 이값이 1.26일때 가속을 안하는 증상이 보임..
values["AccelLimitBandLower"] = 0.0
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, jerk_u, jerk_l, CS):
enabled = enabled or CS.softHoldActive > 0
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
a_val, a_raw = 0, 0
else:
a_raw = accel
a_val = np.clip(accel, accel_last - jn, accel_last + jn)
values = {
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
"MainMode_ACC": 1,
"StopReq": 1 if stopping or CS.softHoldActive > 0 else 0,
"aReqValue": a_val,
"aReqRaw": a_raw,
"VSetDis": set_speed,
#"JerkLowerLimit": jerk if enabled else 1,
#"JerkUpperLimit": 3.0,
"JerkLowerLimit": jerk_l if enabled else 1,
"JerkUpperLimit": jerk_u,
"ACC_ObjDist": 1,
#"ObjValid": 0,
#"OBJ_STATUS": 2,
"NSCCOper": 0,
"NSCCOnOff": 2,
"DriveMode": 0,
#"SET_ME_3": 0x3,
"ACC_ObjLatPos": 0x64,
"DISTANCE_SETTING": hud_control.leadDistanceBars, # + 5,
"InfoDisplay": 4 if stopping and CS.out.cruiseState.standstill else 0,
}
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
ret = []
values = {
}
ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values))
blink = 0
if left_blink:
blink = 3
elif right_blink:
blink = 4
values = {
"BLINKER_CONTROL": blink,
}
ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values))
return ret
def create_fca_warning_light(CP, packer, CAN, frame):
ret = []
if CP.flags & HyundaiFlags.CAMERA_SCC.value:
return ret
if frame % 2 == 0:
values = {
'AEB_SETTING': 0x1, # show AEB disabled icon
'SET_ME_2': 0x2,
'SET_ME_FF': 0xff,
'SET_ME_FC': 0xfc,
'SET_ME_9': 0x9,
#'DATA102': 1,
}
ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
return ret
def create_tcs_messages(packer, CAN, CS):
ret = []
if CS.tcs_info_373 is not None:
values = copy.copy(CS.tcs_info_373)
values["DriverBraking"] = 0
values["DriverBrakingLowSens"] = 0
ret.append(packer.make_can_msg("TCS", CAN.CAM, values))
return ret
def forward_button_message(packer, CAN, frame, CS, cruise_button, MainMode_ACC_trigger, LFA_trigger):
ret = []
if frame % 2 == 0:
if CS.cruise_buttons_msg is not None:
values = copy.copy(CS.cruise_buttons_msg)
cruise_button_driver = values["CRUISE_BUTTONS"]
if cruise_button_driver == 0:
values["CRUISE_BUTTONS"] = cruise_button
if MainMode_ACC_trigger > 0:
#values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
pass
elif LFA_trigger > 0:
values["LFA_BTN"] = 1
ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
return ret
def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle, left_lane_warning, right_lane_warning, canfd_debug, MainMode_ACC_trigger, LFA_trigger):
ret = []
if CP.flags & HyundaiFlags.CAMERA_SCC.value:
if frame % 2 == 0:
if CS.adrv_info_160 is not None:
values = copy.copy(CS.adrv_info_160)
#values["NEW_SIGNAL_1"] = 0 # steer_temp관련없음, 계기판에러
#values["SET_ME_9"] = 17 # steer_temp관련없음, 계기판에러
#values["SET_ME_2"] = 0 #커멘트해도 steer_temp에러남, 2값은 콤마에서 찾은거니...
#values["DATA102"] = 0 # steer_temp관련없음
ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
if CS.cruise_buttons_msg is not None:
values = copy.copy(CS.cruise_buttons_msg)
if MainMode_ACC_trigger > 0:
values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
elif LFA_trigger > 0:
values["LFA_BTN"] = 1
ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
if frame % 5 == 0:
if CS.adrv_info_161 is not None:
main_enabled = CS.out.cruiseState.available
cruise_enabled = CC.enabled
lat_enabled = CS.out.latEnabled
lat_active = CC.latActive
nav_active = hud_control.activeCarrot > 1
# hdpuse carrot
hdp_use = int(Params().get("HDPuse"))
hdp_active = False
if hdp_use == 1:
hdp_active = cruise_enabled and nav_active
elif hdp_use == 2:
hdp_active = cruise_enabled
# hdpuse carrot
values = copy.copy(CS.adrv_info_161)
#print("adrv_info_161 = ", CS.adrv_info_161)
values["SETSPEED"] = (6 if hdp_active else 3 if cruise_enabled else 1) if main_enabled else 0
values["SETSPEED_HUD"] = (5 if hdp_active else 3 if cruise_enabled else 1) if main_enabled else 0
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
values["vSetDis"] = int(set_speed_in_units + 0.5)
values["DISTANCE"] = 4 if hdp_active else hud_control.leadDistanceBars
values["DISTANCE_LEAD"] = 2 if cruise_enabled and hud_control.leadVisible else 1 if main_enabled and hud_control.leadVisible else 0
values["DISTANCE_CAR"] = 3 if hdp_active else 2 if cruise_enabled else 1 if main_enabled else 0
values["DISTANCE_SPACING"] = 5 if hdp_active else 1 if cruise_enabled else 0
values["TARGET"] = 1 if main_enabled else 0
values["TARGET_DISTANCE"] = int(hud_control.leadDistance)
values["BACKGROUND"] = 6 if CS.paddle_button_prev > 0 else 1 if cruise_enabled else 3 if main_enabled else 7
values["CENTERLINE"] = 1 if lat_enabled else 0
values["CAR_CIRCLE"] = 2 if hdp_active else 1 if cruise_enabled else 0
values["NAV_ICON"] = 2 if nav_active else 0
values["HDA_ICON"] = 5 if hdp_active else 2 if cruise_enabled else 1 if main_enabled else 0
values["LFA_ICON"] = 5 if hdp_active else 2 if lat_active else 1 if lat_enabled else 0
values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 0
values["FCA_ALT_ICON"] = 0
if values["ALERTS_2"] in [1, 2, 5, 10, 21, 22]: # 10,21,22: 운전자모니터 알람/경고
values["ALERTS_2"] = 0
values["DAW_ICON"] = 0
values["SOUNDS_1"] = 0 # 운전자모니터경고음.
values["SOUNDS_2"] = 0 # 2: STEER중지 경고후에도 사운드가 나옴.
values["SOUNDS_4"] = 0 # 차선변경알림? 에이 그냥0으로..
if values["ALERTS_3"] in [3, 4, 13, 17, 19, 26, 7, 8, 9, 10]:
values["ALERTS_3"] = 0
values["SOUNDS_3"] = 0
if values["ALERTS_5"] in [1, 2, 4, 5]:
values["ALERTS_5"] = 0
if values["ALERTS_5"] in [11] and CS.softHoldActive == 0:
values["ALERTS_5"] = 0
curvature = round(CS.out.steeringAngleDeg / 3)
values["LANELINE_CURVATURE"] = (min(abs(curvature), 15) + (-1 if curvature < 0 else 0)) if lat_active else 0
values["LANELINE_CURVATURE_DIRECTION"] = 1 if curvature < 0 and lat_active else 0
# lane_color = 6 if lat_active else 2
lane_color = 2 # 6: green, 2: white, 4: yellow
if hud_control.leftLaneDepart:
values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1
else:
values["LANELINE_LEFT"] = lane_color if hud_control.leftLaneVisible else 0
if hud_control.rightLaneDepart:
values["LANELINE_RIGHT"] = 4 if (frame // 50) % 2 == 0 else 1
else:
values["LANELINE_RIGHT"] = lane_color if hud_control.rightLaneVisible else 0
#values["LANELINE_LEFT_POSITION"] = 15
#values["LANELINE_RIGHT_POSITION"] = 15
values["LCA_LEFT_ARROW"] = 2 if CS.out.leftBlinker else 0
values["LCA_RIGHT_ARROW"] = 2 if CS.out.rightBlinker else 0
values["LCA_LEFT_ICON"] = 1 if CS.out.leftBlindspot else 2
values["LCA_RIGHT_ICON"] = 1 if CS.out.rightBlindspot else 2
ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values))
if CS.adrv_info_200 is not None:
values = copy.copy(CS.adrv_info_200)
values["TauGapSet"] = hud_control.leadDistanceBars
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values))
if CS.adrv_info_1ea is not None:
values = copy.copy(CS.adrv_info_1ea)
#values["HDA_MODE1"] = 8
#values["HDA_MODE2"] = 1
if values['LF_DETECT'] == 0 and hud_control.leadLeftDist > 0:
values['LF_DETECT'] = 3 if hud_control.leadLeftDist > 30 else 4
values['LF_DETECT_DISTANCE'] = hud_control.leadLeftDist
values['LF_DETECT_LATERAL'] = hud_control.leadLeftLat
if values['RF_DETECT'] == 0 and hud_control.leadRightDist > 0:
values['RF_DETECT'] = 3 if hud_control.leadRightDist > 30 else 4
values['RF_DETECT_DISTANCE'] = hud_control.leadRightDist
values['RF_DETECT_LATERAL'] = hud_control.leadRightLat
"""
if values['LR_DETECT'] == 0 and hud_control.leadLeftDist2 > 0:
values['LR_DETECT'] = 4
values['LR_DETECT_DISTANCE'] = 2
values['LR_DETECT_LATERAL'] = hud_control.leadLeftLat2
if values['RR_DETECT'] == 0 and hud_control.leadRightDist2 > 0:
values['RR_DETECT'] = 4
values['RR_DETECT_DISTANCE'] = 2
values['RR_DETECT_LATERAL'] = hud_control.leadRightLat2
"""
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))
if CS.adrv_info_162 is not None:
values = copy.copy(CS.adrv_info_162)
if hud_control.leadDistance > 0:
values["FF_DISTANCE"] = hud_control.leadDistance
#values["FF_DETECT"] = 11 if hud_control.leadRelSpeed > -0.1 else 12 # bicycle
#values["FF_DETECT"] = 5 if hud_control.leadRelSpeed > -0.1 else 6 # truck
ff_type = 3 if hud_control.leadRadar == 1 else 13
values["FF_DETECT"] = ff_type if hud_control.leadRelSpeed > -0.1 else ff_type + 1
#values["FF_DETECT_LAT"] = - hud_control.leadDPath
if True:
if values['LF_DETECT'] == 0 and hud_control.leadLeftDist > 0:
values['LF_DETECT'] = 3 if hud_control.leadLeftDist > 30 else 4
values['LF_DETECT_DISTANCE'] = hud_control.leadLeftDist
values['LF_DETECT_LATERAL'] = hud_control.leadLeftLat
if values['RF_DETECT'] == 0 and hud_control.leadRightDist > 0:
values['RF_DETECT'] = 3 if hud_control.leadRightDist > 30 else 4
values['RF_DETECT_DISTANCE'] = hud_control.leadRightDist
values['RF_DETECT_LATERAL'] = hud_control.leadRightLat
if values['LR_DETECT'] == 0 and hud_control.leadLeftDist2 > 0:
values['LR_DETECT'] = 4
values['LR_DETECT_DISTANCE'] = 2
values['LR_DETECT_LATERAL'] = hud_control.leadLeftLat2
if values['RR_DETECT'] == 0 and hud_control.leadRightDist2 > 0:
values['RR_DETECT'] = 4
values['RR_DETECT_DISTANCE'] = 2
values['RR_DETECT_LATERAL'] = hud_control.leadRightLat2
else:
sensors = [
('lf', 'LF_DETECT'),
('rf', 'RF_DETECT'),
('lr', 'LR_DETECT'),
('rr', 'RR_DETECT')
]
for sensor_key, detect_key in sensors:
distance = getattr(CS, f"{sensor_key}_distance")
if distance > 0:
values[detect_key] = 3 if distance > 30 else 4
"""
values["FAULT_FCA"] = 0
values["FAULT_LSS"] = 0
values["FAULT_LFA"] = 0
values["FAULT_LCA"] = 0
values["FAULT_DAS"] = 0
values["FAULT_HDA"] = 0
"""
if (left_lane_warning and not CS.out.leftBlinker) or (right_lane_warning and not CS.out.rightBlinker):
values["VIBRATE"] = 1
ret.append(packer.make_can_msg("CCNC_0x162", CAN.ECAN, values))
if canfd_debug > 0:
if frame % 20 == 0: # 아직 시험중..
if CS.hda_info_4a3 is not None:
values = copy.copy(CS.hda_info_4a3)
#if canfd_debug == 1:
values["SIGNAL_0"] = 5
values["NEW_SIGNAL_1"] = 4
values["SPEED_LIMIT"] = 80
values["NEW_SIGNAL_3"] = 154
values["NEW_SIGNAL_4"] = 9
values["NEW_SIGNAL_5"] = 0
values["NEW_SIGNAL_6"] = 256
ret.append(packer.make_can_msg("HDA_INFO_4A3", CAN.CAM, values))
return ret
def create_adrv_messages(CP, packer, CAN, frame):
# messages needed to car happy after disabling
# the ADAS Driving ECU to do longitudinal control
ret = []
if not CP.flags & HyundaiFlags.CAMERA_SCC.value:
values = {}
ret.extend(create_fca_warning_light(CP, packer, CAN, frame))
if frame % 5 == 0:
values = {
'HDA_MODE1': 0x8,
'HDA_MODE2': 0x1,
#'SET_ME_1C': 0x1c,
'SET_ME_FF': 0xff,
#'SET_ME_TMP_F': 0xf,
#'SET_ME_TMP_F_2': 0xf,
#'DATA26': 1, #1
#'DATA32': 5, #5
}
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))
values = {
'SET_ME_E1': 0xe1,
#'SET_ME_3A': 0x3a,
'TauGapSet' : 1,
'NEW_SIGNAL_2': 3,
}
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", CAN.ECAN, values))
if frame % 100 == 0:
values = {
'SET_ME_22': 0x22,
'SET_ME_41': 0x41,
}
ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values))
return ret
## carrot
def alt_cruise_buttons(packer, CP, CAN, buttons, cruise_btns_msg, cnt):
cruise_btns_msg["CRUISE_BUTTONS"] = buttons
cruise_btns_msg["COUNTER"] = (cruise_btns_msg["COUNTER"] + 1 + cnt) % 256
bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
return packer.make_can_msg("CRUISE_BUTTONS_ALT", bus, cruise_btns_msg)
def hkg_can_fd_checksum(address: int, sig, d: bytearray) -> int:
crc = 0
for i in range(2, len(d)):
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ d[i]]) & 0xFFFF
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ ((address >> 0) & 0xFF)]) & 0xFFFF
crc = ((crc << 8) ^ CRC16_XMODEM[(crc >> 8) ^ ((address >> 8) & 0xFF)]) & 0xFFFF
if len(d) == 8:
crc ^= 0x5F29
elif len(d) == 16:
crc ^= 0x041D
elif len(d) == 24:
crc ^= 0x819D
elif len(d) == 32:
crc ^= 0x9F5B
return crc

284
opendbc_repo/opendbc/car/hyundai/interface.py Normal file
View File

@@ -0,0 +1,284 @@
from opendbc.car import Bus, get_safety_config, structs
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_RADAR_SCC_CAR, \
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, \
UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, HyundaiExtFlags
from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR
from opendbc.car.interfaces import CarInterfaceBase
from opendbc.car.disable_ecu import disable_ecu
from opendbc.car.hyundai.carcontroller import CarController
from opendbc.car.hyundai.carstate import CarState
from opendbc.car.hyundai.radar_interface import RadarInterface
from openpilot.common.params import Params
ButtonType = structs.CarState.ButtonEvent.Type
Ecu = structs.CarParams.Ecu
# Cancel button can sometimes be ACC pause/resume button, main button can also enable on some cars
ENABLE_BUTTONS = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.cancel, ButtonType.mainCruise)
SteerControlType = structs.CarParams.SteerControlType
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
params = Params()
camera_scc = params.get_int("HyundaiCameraSCC")
if camera_scc > 0:
ret.flags |= HyundaiFlags.CAMERA_SCC.value
print("$$$CAMERA_SCC toggled...")
ret.brand = "hyundai"
cam_can = CanBus(None, fingerprint).CAM if camera_scc == 0 else 1
hda2 = False #0x50 in fingerprint[cam_can] or 0x110 in fingerprint[cam_can]
hda2 = hda2 or params.get_int("CanfdHDA2") > 0
CAN = CanBus(None, fingerprint, hda2)
if params.get_int("CanfdDebug") == -1:
ret.flags |= HyundaiFlags.ANGLE_CONTROL.value
if ret.flags & HyundaiFlags.CANFD:
# Shared configuration for CAN-FD cars
ret.alphaLongitudinalAvailable = True #candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR)
#ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN]
ret.enableBsm = 0x1ba in fingerprint[CAN.ECAN] # BLINDSPOTS_REAR_CORNERS 0x1ba(442)
if 0x105 in fingerprint[CAN.ECAN]:
ret.flags |= HyundaiFlags.HYBRID.value
if 203 in fingerprint[CAN.CAM]: # LFA_ALT
print("##### Anglecontrol detected (LFA_ALT)")
ret.flags |= HyundaiFlags.ANGLE_CONTROL.value
print("ACAN=", fingerprint[CAN.ACAN])
if 0x210 in fingerprint[CAN.ACAN]:
print("##### Radar Group 1 detected (0x210)")
ret.extFlags |= HyundaiExtFlags.RADAR_GROUP1.value
# detect HDA2 with ADAS Driving ECU
if hda2:
print("$$$CANFD HDA2")
ret.flags |= HyundaiFlags.CANFD_HDA2.value
if camera_scc > 0:
if 0x110 in fingerprint[CAN.ACAN]:
ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
print("$$$CANFD ALT_STEERING1")
else:
if 0x110 in fingerprint[CAN.CAM]: # 0x110(272): LKAS_ALT
ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
print("$$$CANFD ALT_STEERING1")
## carrot_todo: sorento:
if 0x2a4 not in fingerprint[CAN.CAM]: # 0x2a4(676): CAM_0x2a4
ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
print("$$$CANFD ALT_STEERING2")
## carrot: canival 4th, no 0x1cf
if 0x1cf not in fingerprint[CAN.ECAN]: # 0x1cf(463): CRUISE_BUTTONS
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
print("$$$CANFD ALT_BUTTONS")
else:
# non-HDA2
print("$$$CANFD non HDA2")
if 0x1cf not in fingerprint[CAN.ECAN]:
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
print("$$$CANFD ALT_BUTTONS")
#if not ret.flags & HyundaiFlags.RADAR_SCC:
# ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value
# print("$$$CANFD CAMERA_SCC")
# Some HDA2 cars have alternative messages for gear checks
# ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead
if 0x40 in fingerprint[CAN.ECAN]: # 0x40(64): GEAR_ALT
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value
print("$$$CANFD ALT_GEARS")
elif 69 in fingerprint[CAN.ECAN]: # Special case
ret.extFlags |= HyundaiExtFlags.CANFD_GEARS_69.value
print("$$$CANFD GEARS_69")
elif 112 in fingerprint[CAN.ECAN]: # carrot: eGV70
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS_2.value
print("$$$CANFD ALT_GEARS_2")
elif 0x130 in fingerprint[CAN.ECAN]: # 0x130(304): GEAR_SHIFTER
print("$$$CANFD GEAR_SHIFTER present")
else:
ret.extFlags |= HyundaiExtFlags.CANFD_GEARS_NONE.value
print("$$$CANFD GEARS_NONE")
cfgs = [get_safety_config(structs.CarParams.SafetyModel.hyundaiCanfd), ]
if CAN.ECAN >= 4:
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
ret.safetyConfigs = cfgs
if ret.flags & HyundaiFlags.CANFD_HDA2:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_LKA_STEERING.value
if ret.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT.value
if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ALT_BUTTONS.value
if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAMERA_SCC.value
else:
# Shared configuration for non CAN-FD cars
ret.alphaLongitudinalAvailable = True #candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR)
ret.enableBsm = 0x58b in fingerprint[0]
print(f"$$$ enableBsm = {ret.enableBsm}")
# Send LFA message on cars with HDA
if 0x485 in fingerprint[2]:
ret.flags |= HyundaiFlags.SEND_LFA.value
print("$$$SEND_LFA")
# These cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
if 0x38d in fingerprint[0] or 0x38d in fingerprint[2]:
ret.flags |= HyundaiFlags.USE_FCA.value
print("$$$USE_FCA")
if ret.flags & HyundaiFlags.LEGACY:
# these cars require a special panda safety mode due to missing counters and checksums in the messages
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hyundaiLegacy)]
print("$$$Legacy Safety Model")
else:
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hyundai, 0)]
if ret.flags & HyundaiFlags.CAMERA_SCC:
ret.safetyConfigs[0].safetyParam |= HyundaiSafetyFlags.CAMERA_SCC.value
print("$$$CAMERA_SCC")
# Common lateral control setup
ret.centerToFront = ret.wheelbase * 0.4
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.4
if ret.flags & HyundaiFlags.ANGLE_CONTROL:
ret.steerControlType = SteerControlType.angle
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if ret.flags & HyundaiFlags.ALT_LIMITS:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.ALT_LIMITS.value
# Common longitudinal control setup
ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or Bus.radar not in DBC[ret.carFingerprint]
ret.openpilotLongitudinalControl = alpha_long and ret.alphaLongitudinalAvailable
# carrot, if camera_scc enabled, enable openpilotLongitudinalControl
if ret.flags & HyundaiFlags.CAMERA_SCC.value or params.get_int("EnableRadarTracks") > 0:
ret.radarUnavailable = False
ret.openpilotLongitudinalControl = True if camera_scc != 3 else False
print(f"$$$OenpilotLongitudinalControl = True, CAMERA_SCC({ret.flags & HyundaiFlags.CAMERA_SCC.value}) or RadarTracks{params.get_int('EnableRadarTracks')}")
else:
print(f"$$$OenpilotLongitudinalControl = {alpha_long}")
#ret.radarUnavailable = False # TODO: canfd... carrot, hyundai cars have radar
ret.radarTimeStep = 0.05 #if params.get_int("EnableRadarTracks") > 0 else 0.02
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.startingState = False # True # carrot
ret.vEgoStarting = 0.1
ret.startAccel = 1.0
ret.longitudinalActuatorDelay = 0.5
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [1.]
ret.longitudinalTuning.kf = 1.0
# *** feature detection ***
if ret.flags & HyundaiFlags.CANFD:
print(f"$$$$$ CanFD ECAN = {CAN.ECAN}")
if 0x1fa in fingerprint[CAN.ECAN]:
ret.extFlags |= HyundaiExtFlags.NAVI_CLUSTER.value
print("$$$$ NaviCluster = True")
else:
print("$$$$ NaviCluster = False")
else:
if 1348 in fingerprint[0]:
ret.extFlags |= HyundaiExtFlags.NAVI_CLUSTER.value
print("$$$$ NaviCluster = True")
if 1157 in fingerprint[0] or 1157 in fingerprint[2]:
ret.extFlags |= HyundaiExtFlags.HAS_LFAHDA.value
print("$$$$ HasLFAHDA")
if 1007 in fingerprint[0]:
print("#### cruiseButtonAlt")
print(f"$$$$ enableBsm = {ret.enableBsm}")
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.LONG.value
if ret.flags & HyundaiFlags.HYBRID:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.HYBRID_GAS.value
elif ret.flags & HyundaiFlags.EV:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.EV_GAS.value
elif ret.flags & HyundaiFlags.FCEV:
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.FCEV_GAS.value
# Car specific configuration overrides
if candidate == CAR.KIA_OPTIMA_G4_FL:
ret.steerActuatorDelay = 0.2
# Dashcam cars are missing a test route, or otherwise need validation
# TODO: Optima Hybrid 2017 uses a different SCC12 checksum
#ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, }
return ret
@staticmethod
def init(CP, can_recv, can_send):
Params().put('LongitudinalPersonalityMax', "4")
if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC):
addr, bus = 0x7d0, 0
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, CanBus(CP).ECAN
disable_ecu(can_recv, can_send, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
params = Params()
if params.get_int("EnableRadarTracks") > 0 and not CP.flags & HyundaiFlags.CANFD:
result = enable_radar_tracks(CP, can_recv, can_send)
params.put_bool("EnableRadarTracksResult", result)
# for blinkers
if CP.flags & HyundaiFlags.ENABLE_BLINKERS:
disable_ecu(can_recv, can_send, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01')
def enable_radar_tracks(CP, logcan, sendcan):
from opendbc.car.isotp_parallel_query import IsoTpParallelQuery
print("################ Try To Enable Radar Tracks ####################")
ret = False
sccBus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC.value else 0
rdr_fw = None
rdr_fw_address = 0x7d0 #
try:
try:
query = IsoTpParallelQuery(sendcan, logcan, sccBus, [rdr_fw_address], [b'\x10\x07'], [b'\x50\x07'])
for addr, dat in query.get_data(0.1).items(): # pylint: disable=unused-variable
print("ecu write data by id ...")
new_config = b"\x00\x00\x00\x01\x00\x01"
#new_config = b"\x00\x00\x00\x00\x00\x01"
dataId = b'\x01\x42'
WRITE_DAT_REQUEST = b'\x2e'
WRITE_DAT_RESPONSE = b'\x68'
query = IsoTpParallelQuery(sendcan, logcan, sccBus, [rdr_fw_address], [WRITE_DAT_REQUEST+dataId+new_config], [WRITE_DAT_RESPONSE])
result = query.get_data(0)
print("result=", result)
ret = True
break
except Exception as e:
print(f"Failed : {e}")
except Exception as e:
print("############## Failed to enable tracks" + str(e))
print("################ END Try to enable radar tracks")
return ret

247
opendbc_repo/opendbc/car/hyundai/radar_interface.py Normal file
View File

@@ -0,0 +1,247 @@
import math
from opendbc.can import CANParser
from opendbc.car import Bus, structs
from opendbc.car.interfaces import RadarInterfaceBase
from opendbc.car.hyundai.values import DBC, HyundaiFlags, HyundaiExtFlags
from openpilot.common.params import Params
from opendbc.car.hyundai.hyundaicanfd import CanBus
from openpilot.common.filter_simple import MyMovingAverage
SCC_TID = 0
RADAR_START_ADDR = 0x500
RADAR_MSG_COUNT = 32
RADAR_START_ADDR_CANFD1 = 0x210
RADAR_MSG_COUNT1 = 16
RADAR_START_ADDR_CANFD2 = 0x3A5 # Group 2, Group 1: 0x210 2개씩있어서 일단 보류.
RADAR_MSG_COUNT2 = 32
# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
def get_radar_can_parser(CP, radar_tracks, msg_start_addr, msg_count):
if not radar_tracks:
return None
#if Bus.radar not in DBC[CP.carFingerprint]:
# return None
print("RadarInterface: RadarTracks...")
if CP.flags & HyundaiFlags.CANFD:
CAN = CanBus(CP)
messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(msg_start_addr, msg_start_addr + msg_count)]
return CANParser('hyundai_canfd_radar_generated', messages, CAN.ACAN)
else:
messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(msg_start_addr, msg_start_addr + msg_count)]
#return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 1)
return CANParser('hyundai_kia_mando_front_radar_generated', messages, 1)
def get_radar_can_parser_scc(CP):
CAN = CanBus(CP)
if CP.flags & HyundaiFlags.CANFD:
messages = [("SCC_CONTROL", 50)]
bus = CAN.ECAN
else:
messages = [("SCC11", 50)]
bus = CAN.ECAN
print("$$$$$$$$ ECAN = ", CAN.ECAN)
bus = CAN.CAM if CP.flags & HyundaiFlags.CAMERA_SCC else bus
return CANParser(DBC[CP.carFingerprint][Bus.pt], messages, bus)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.canfd = True if CP.flags & HyundaiFlags.CANFD else False
self.radar_group1 = False
if self.canfd:
if CP.extFlags & HyundaiExtFlags.RADAR_GROUP1.value:
self.radar_start_addr = RADAR_START_ADDR_CANFD1
self.radar_msg_count = RADAR_MSG_COUNT1
self.radar_group1 = True
else:
self.radar_start_addr = RADAR_START_ADDR_CANFD2
self.radar_msg_count = RADAR_MSG_COUNT2
else:
self.radar_start_addr = RADAR_START_ADDR
self.radar_msg_count = RADAR_MSG_COUNT
self.params = Params()
self.radar_tracks = self.params.get_int("EnableRadarTracks") >= 1
self.updated_tracks = set()
self.updated_scc = set()
self.rcp_tracks = get_radar_can_parser(CP, self.radar_tracks, self.radar_start_addr, self.radar_msg_count)
self.rcp_scc = get_radar_can_parser_scc(CP)
self.trigger_msg_scc = 416 if self.canfd else 0x420
self.trigger_msg_tracks = self.radar_start_addr + self.radar_msg_count - 1
self.track_id = 0
self.radar_off_can = CP.radarUnavailable
self.vRel_last = 0
self.dRel_last = 0
# Initialize pts
total_tracks = self.radar_msg_count * ( 2 if self.radar_group1 else 1)
for track_id in range(total_tracks):
t_id = track_id + 32
self.pts[t_id] = structs.RadarData.RadarPoint()
self.pts[t_id].measured = False
self.pts[t_id].trackId = t_id
self.pts[SCC_TID] = structs.RadarData.RadarPoint()
self.pts[SCC_TID].trackId = SCC_TID
self.frame = 0
def update(self, can_strings):
self.frame += 1
if self.radar_off_can or (self.rcp_tracks is None and self.rcp_scc is None):
return super().update(None)
if self.rcp_scc is not None:
vls_s = self.rcp_scc.update(can_strings)
self.updated_scc.update(vls_s)
if not self.radar_tracks and self.frame % 5 == 0:
self._update_scc(self.updated_scc)
self.updated_scc.clear()
ret = structs.RadarData()
if not self.rcp_scc.can_valid:
ret.errors.canError = True
ret.points = list(self.pts.values())
return ret
if self.radar_tracks and self.rcp_tracks is not None:
vls_t = self.rcp_tracks.update(can_strings)
self.updated_tracks.update(vls_t)
if self.trigger_msg_tracks in self.updated_tracks:
self._update(self.updated_tracks)
self._update_scc(self.updated_scc)
self.updated_scc.clear()
self.updated_tracks.clear()
ret = structs.RadarData()
if not self.rcp_tracks.can_valid:
ret.errors.canError = True
ret.points = list(self.pts.values())
return ret
return None
def _update(self, updated_messages):
t_id = 32
for addr in range(self.radar_start_addr, self.radar_start_addr + self.radar_msg_count):
msg = self.rcp_tracks.vl[f"RADAR_TRACK_{addr:x}"]
if self.radar_group1:
valid = msg['VALID_CNT1'] > 10
elif self.canfd:
valid = msg['VALID_CNT'] > 10
else:
valid = msg['STATE'] in (3, 4)
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
elif self.radar_group1:
self.pts[t_id].dRel = msg['LONG_DIST1']
self.pts[t_id].yRel = msg['LAT_DIST1']
self.pts[t_id].vRel = msg['REL_SPEED1']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL1']
self.pts[t_id].yvRel = msg['LAT_SPEED1']
elif self.canfd:
self.pts[t_id].dRel = msg['LONG_DIST']
self.pts[t_id].yRel = msg['LAT_DIST']
self.pts[t_id].vRel = msg['REL_SPEED']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL']
self.pts[t_id].yvRel = msg['LAT_SPEED']
else:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[t_id].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[t_id].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[t_id].vRel = msg['REL_SPEED']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL']
self.pts[t_id].yvRel = 0.0
t_id += 1
# radar group1은 하나의 msg에 2개의 레이더가 들어있음.
if self.radar_group1:
for addr in range(self.radar_start_addr, self.radar_start_addr + self.radar_msg_count):
msg = self.rcp_tracks.vl[f"RADAR_TRACK_{addr:x}"]
valid = msg['VALID_CNT2'] > 10
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = msg['LONG_DIST2']
self.pts[t_id].yRel = msg['LAT_DIST2']
self.pts[t_id].vRel = msg['REL_SPEED2']
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = msg['REL_ACCEL2']
self.pts[t_id].yvRel = msg['LAT_SPEED2']
t_id += 1
def _update_scc(self, updated_messages):
cpt = self.rcp_scc.vl
t_id = SCC_TID
if self.canfd:
dRel = cpt["SCC_CONTROL"]['ACC_ObjDist']
vRel = cpt["SCC_CONTROL"]['ACC_ObjRelSpd']
new_pts = abs(dRel - self.dRel_last) > 3 or abs(vRel - self.vRel_last) > 1
vLead = vRel + self.v_ego
valid = 0 < dRel < 150 and not new_pts #cpt["SCC_CONTROL"]['OBJ_STATUS'] and dRel < 150
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = dRel
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = vRel
self.pts[t_id].vLead = vLead
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0 #float('nan')
else:
dRel = cpt["SCC11"]['ACC_ObjDist']
vRel = cpt["SCC11"]['ACC_ObjRelSpd']
new_pts = abs(dRel - self.dRel_last) > 3 or abs(vRel - self.vRel_last) > 1
vLead = vRel + self.v_ego
valid = cpt["SCC11"]['ACC_ObjStatus'] and dRel < 150 and not new_pts
self.pts[t_id].measured = bool(valid)
if not valid:
self.pts[t_id].dRel = 0
self.pts[t_id].yRel = 0
self.pts[t_id].vRel = 0
self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0
else:
self.pts[t_id].dRel = dRel
self.pts[t_id].yRel = -cpt["SCC11"]['ACC_ObjLatPos'] # in car frame's y axis, left is negative
self.pts[t_id].vRel = vRel
self.pts[t_id].vLead = vLead
self.pts[t_id].aRel = float('nan')
self.pts[t_id].yvRel = 0 #float('nan')
self.dRel_last = dRel
self.vRel_last = vRel

0
opendbc_repo/opendbc/car/hyundai/tests/__init__.py Normal file
View File

21
opendbc_repo/opendbc/car/hyundai/tests/print_platform_codes.py Executable file
View File

@@ -0,0 +1,21 @@
#!/usr/bin/env python3
from opendbc.car.structs import CarParams
from opendbc.car.hyundai.values import PLATFORM_CODE_ECUS, get_platform_codes
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
if __name__ == "__main__":
for car_model, ecus in FW_VERSIONS.items():
print()
print(car_model)
for ecu in sorted(ecus):
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
platform_codes = get_platform_codes(ecus[ecu])
codes = {code for code, _ in platform_codes}
dates = {date for _, date in platform_codes if date is not None}
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
print(f' Codes: {codes}')
print(f' Dates: {dates}')

247
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py Normal file
View File

@@ -0,0 +1,247 @@
from hypothesis import settings, given, strategies as st
import pytest
from opendbc.car import gen_empty_fingerprint
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR
from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \
UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
HyundaiFlags, get_platform_codes, HyundaiSafetyFlags
from opendbc.car.hyundai.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
# Some platforms have date codes in a different format we don't yet parse (or are missing).
# For now, assert list of expected missing date cars
NO_DATES_PLATFORMS = {
# CAN FD
CAR.KIA_SPORTAGE_5TH_GEN,
CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN,
CAR.HYUNDAI_TUCSON_4TH_GEN,
# CAN
CAR.HYUNDAI_ELANTRA,
CAR.HYUNDAI_ELANTRA_GT_I30,
CAR.KIA_CEED,
CAR.KIA_FORTE,
CAR.KIA_OPTIMA_G4,
CAR.KIA_OPTIMA_G4_FL,
CAR.KIA_SORENTO,
CAR.HYUNDAI_KONA,
CAR.HYUNDAI_KONA_EV,
CAR.HYUNDAI_KONA_EV_2022,
CAR.HYUNDAI_KONA_HEV,
CAR.HYUNDAI_SONATA_LF,
CAR.HYUNDAI_VELOSTER,
CAR.HYUNDAI_KONA_2022,
}
CANFD_EXPECTED_ECUS = {Ecu.fwdCamera, Ecu.fwdRadar}
class TestHyundaiFingerprint:
def test_feature_detection(self):
# LKA steering
for lka_steering in (True, False):
fingerprint = gen_empty_fingerprint()
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)
assert bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) == lka_steering
# radar available
for radar in (True, False):
fingerprint = gen_empty_fingerprint()
if radar:
fingerprint[1][RADAR_START_ADDR] = 8
CP = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False)
assert CP.radarUnavailable != radar
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)
assert bool(CP.flags & HyundaiFlags.ALT_LIMITS) == bool(CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.ALT_LIMITS)
def test_can_features(self):
# Test no EV/HEV in any gear lists (should all use ELECT_GEAR)
assert set.union(*CAN_GEARS.values()) & (HYBRID_CAR | EV_CAR) == set()
# Test CAN FD car not in CAN feature lists
can_specific_feature_list = set.union(*CAN_GEARS.values(), *CHECKSUM.values(), LEGACY_SAFETY_MODE_CAR, UNSUPPORTED_LONGITUDINAL_CAR, CAMERA_SCC_CAR)
for car_model in CANFD_CAR:
assert car_model not in can_specific_feature_list, "CAN FD car unexpectedly found in a CAN feature list"
def test_hybrid_ev_sets(self):
assert HYBRID_CAR & EV_CAR == set(), "Shared cars between hybrid and EV"
assert CANFD_CAR & HYBRID_CAR == set(), "Hard coding CAN FD cars as hybrid is no longer supported"
def test_canfd_ecu_whitelist(self):
# Asserts only expected Ecus can exist in database for CAN-FD cars
for car_model in CANFD_CAR:
ecus = {fw[0] for fw in FW_VERSIONS[car_model].keys()}
ecus_not_in_whitelist = ecus - CANFD_EXPECTED_ECUS
ecu_strings = ", ".join([f"Ecu.{ecu}" for ecu in ecus_not_in_whitelist])
assert len(ecus_not_in_whitelist) == 0, \
f"{car_model}: Car model has unexpected ECUs: {ecu_strings}"
def test_blacklisted_parts(self, subtests):
# Asserts no ECUs known to be shared across platforms exist in the database.
# Tucson having Santa Cruz camera and EPS for example
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
if car_model == CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN:
pytest.skip("Skip checking Santa Cruz for its parts")
for code, _ in get_platform_codes(ecus[(Ecu.fwdCamera, 0x7c4, None)]):
if b"-" not in code:
continue
part = code.split(b"-")[1]
assert not part.startswith(b'CW'), "Car has bad part number"
def test_correct_ecu_response_database(self, subtests):
"""
Assert standard responses for certain ECUs, since they can
respond to multiple queries with different data
"""
expected_fw_prefix = HYUNDAI_VERSION_REQUEST_LONG[1:]
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
for ecu, fws in ecus.items():
assert all(fw.startswith(expected_fw_prefix) for fw in fws), \
f"FW from unexpected request in database: {(ecu, fws)}"
@settings(max_examples=100)
@given(data=st.data())
def test_platform_codes_fuzzy_fw(self, data):
"""Ensure function doesn't raise an exception"""
fw_strategy = st.lists(st.binary())
fws = data.draw(fw_strategy)
get_platform_codes(fws)
def test_expected_platform_codes(self, subtests):
# Ensures we don't accidentally add multiple platform codes for a car unless it is intentional
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
for ecu, fws in ecus.items():
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
# Third and fourth character are usually EV/hybrid identifiers
codes = {code.split(b"-")[0][:2] for code, _ in get_platform_codes(fws)}
if car_model == CAR.HYUNDAI_PALISADE:
assert codes == {b"LX", b"ON"}, f"Car has unexpected platform codes: {car_model} {codes}"
elif car_model == CAR.HYUNDAI_KONA_EV and ecu[0] == Ecu.fwdCamera:
assert codes == {b"OE", b"OS"}, f"Car has unexpected platform codes: {car_model} {codes}"
else:
assert len(codes) == 1, f"Car has multiple platform codes: {car_model} {codes}"
# Tests for platform codes, part numbers, and FW dates which Hyundai will use to fuzzy
# fingerprint in the absence of full FW matches:
def test_platform_code_ecus_available(self, subtests):
# TODO: add queries for these non-CAN FD cars to get EPS
no_eps_platforms = CANFD_CAR | {CAR.KIA_SORENTO, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H,
CAR.KIA_OPTIMA_H_G4_FL, CAR.HYUNDAI_SONATA_LF, CAR.HYUNDAI_TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA}
# Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
for platform_code_ecu in PLATFORM_CODE_ECUS:
if platform_code_ecu in (Ecu.fwdRadar, Ecu.eps) and car_model == CAR.HYUNDAI_GENESIS:
continue
if platform_code_ecu == Ecu.eps and car_model in no_eps_platforms:
continue
assert platform_code_ecu in [e[0] for e in ecus]
def test_fw_format(self, subtests):
# Asserts:
# - every supported ECU FW version returns one platform code
# - every supported ECU FW version has a part number
# - expected parsing of ECU FW dates
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
for ecu, fws in ecus.items():
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
codes = set()
for fw in fws:
result = get_platform_codes([fw])
assert 1 == len(result), f"Unable to parse FW: {fw}"
codes |= result
if ecu[0] not in DATE_FW_ECUS or car_model in NO_DATES_PLATFORMS:
assert all(date is None for _, date in codes)
else:
assert all(date is not None for _, date in codes)
if car_model == CAR.HYUNDAI_GENESIS:
pytest.skip("No part numbers for car model")
# 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}"
def test_platform_codes_spot_check(self):
# Asserts basic platform code parsing behavior for a few cases
results = get_platform_codes([b"\xf1\x00DH LKAS 1.1 -150210"])
assert results == {(b"DH", b"150210")}
# Some cameras and all radars do not have dates
results = get_platform_codes([b"\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 "])
assert results == {(b"AEhe-G2000", None)}
results = get_platform_codes([b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 "])
assert results == {(b"CV1-CV000", None)}
results = get_platform_codes([
b"\xf1\x00DH LKAS 1.1 -150210",
b"\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ",
b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ",
])
assert results == {(b"DH", b"150210"), (b"AEhe-G2000", None), (b"CV1-CV000", None)}
results = get_platform_codes([
b"\xf1\x00LX2 MFC AT USA LHD 1.00 1.07 99211-S8100 220222",
b"\xf1\x00LX2 MFC AT USA LHD 1.00 1.08 99211-S8100 211103",
b"\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 190405",
b"\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 190720",
])
assert results == {(b"LX2-S8100", b"220222"), (b"LX2-S8100", b"211103"),
(b"ON-S9100", b"190405"), (b"ON-S9100", b"190720")}
def test_fuzzy_excluded_platforms(self):
# Asserts a list of platforms that will not fuzzy fingerprint with platform codes due to them being shared.
# This list can be shrunk as we combine platforms and detect features
excluded_platforms = {
CAR.GENESIS_G70, # shared platform code, part number, and date
CAR.GENESIS_G70_2020,
}
excluded_platforms |= CANFD_CAR - EV_CAR - CANFD_FUZZY_WHITELIST # shared platform codes
excluded_platforms |= NO_DATES_PLATFORMS # date codes are required to match
platforms_with_shared_codes = set()
for platform, fw_by_addr in FW_VERSIONS.items():
car_fw = []
for ecu, fw_versions in fw_by_addr.items():
ecu_name, addr, sub_addr = ecu
for fw in fw_versions:
car_fw.append(CarParams.CarFw(ecu=ecu_name, fwVersion=fw, address=addr,
subAddress=0 if sub_addr is None else sub_addr))
CP = CarParams(carFw=car_fw)
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS)
if len(matches) == 1:
assert list(matches)[0] == platform
else:
platforms_with_shared_codes.add(platform)
assert platforms_with_shared_codes == excluded_platforms

1010
opendbc_repo/opendbc/car/hyundai/values.py Normal file
View File

File diff suppressed because it is too large Load Diff