IQ.Pilot Prebuilt Release @ 27f668a
This commit is contained in:
IQ.Lvbs CI [bot]
788
artifacts/package_runtime/iqdbc/car/hyundai/carcontroller.py
Normal file
788
artifacts/package_runtime/iqdbc/car/hyundai/carcontroller.py
Normal file
@@ -0,0 +1,788 @@
|
||||
import numpy as np
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
|
||||
from iqdbc.car.lateral import apply_driver_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.hyundai import hyundaicanfd, hyundaican
|
||||
from iqdbc.car.hyundai.carstate import CarState
|
||||
from iqdbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CAR, CAN_GEARS, HyundaiExtFlags
|
||||
from iqdbc.car.interfaces import CarControllerBase
|
||||
from iqdbc.car.vehicle_model import VehicleModel
|
||||
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
from iqpilot.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
|
||||
DRIVER_TORQUE_FILTER_TAU = 0.12
|
||||
PRE_OVERRIDE_PREDICTION_TIME = 0.15
|
||||
PRE_OVERRIDE_START_RATIO = 0.90
|
||||
PRE_OVERRIDE_FULL_RATIO = 1.05
|
||||
PRE_OVERRIDE_RAW_MIN_RATIO = 0.70
|
||||
PRE_OVERRIDE_FILTERED_MIN_RATIO = 0.65
|
||||
PRE_OVERRIDE_MIN_RATE_RATIO = 0.50
|
||||
PRE_OVERRIDE_CONFIRM_FRAMES = 2
|
||||
PRE_OVERRIDE_MAX_TORQUE_DELTA = -10.0
|
||||
LOW_SPEED_ANGLE_RATE_RAMP_SPEED = 15.0 * CV.KPH_TO_MS
|
||||
MID_SPEED_ANGLE_RATE_LIMIT_SPEED = 40.0 * CV.KPH_TO_MS
|
||||
|
||||
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 rate_limit(x, x_last, lo, hi):
|
||||
return float(np.clip(x, x_last + lo, x_last + hi))
|
||||
|
||||
def apply_steer_angle_limits_physics(desired_sw_deg: float,
|
||||
last_sw_deg: float,
|
||||
v_ego: float,
|
||||
steering_sw_deg: float,
|
||||
lat_active: bool,
|
||||
wheelbase_m: float,
|
||||
steer_ratio: float,
|
||||
steer_sw_max_deg: float,
|
||||
model_v2=None) -> float:
|
||||
max_lat_accel = 8.5 # m/s^2
|
||||
max_lat_jerk = 4.0 # m/s^3
|
||||
y_std_1s = 0.1
|
||||
if model_v2 is not None and len(model_v2.position.yStd) > 10:
|
||||
model_y_std_1s = float(model_v2.position.yStd[10])
|
||||
if np.isfinite(model_y_std_1s) and model_y_std_1s >= 0.0:
|
||||
y_std_1s = model_y_std_1s
|
||||
max_sw_rate_deg_per_tick = float(np.interp(y_std_1s, [0.1, 0.2, 0.4], [2.0, 1.5, 0.8]))
|
||||
v = max(float(v_ego), 1.0)
|
||||
|
||||
target_sw = float(np.clip(desired_sw_deg, -steer_sw_max_deg, steer_sw_max_deg))
|
||||
if v_ego < MID_SPEED_ANGLE_RATE_LIMIT_SPEED:
|
||||
# Keep low/mid-speed angle commands quieter without reducing LKAS_ANGLE_MAX_TORQUE.
|
||||
# Allow the angle, but slow the arrival: 0~15 kph ramps 0.8->1.1 deg/tick,
|
||||
# then 15~40 kph tapers 1.1->0.8 deg/tick. Above 40 kph, physics limits take over.
|
||||
low_mid_speed_cap = float(np.interp(v_ego,
|
||||
[0.0, LOW_SPEED_ANGLE_RATE_RAMP_SPEED, MID_SPEED_ANGLE_RATE_LIMIT_SPEED],
|
||||
[0.8, 1.1, 0.8]))
|
||||
max_sw_rate_deg_per_tick = min(max_sw_rate_deg_per_tick, low_mid_speed_cap)
|
||||
|
||||
target_rw = target_sw / steer_ratio
|
||||
last_rw = float(last_sw_deg) / steer_ratio
|
||||
|
||||
# --- accel limit ---
|
||||
rw_max_rad = np.arctan((max_lat_accel * wheelbase_m) / (v * v))
|
||||
rw_max = float(np.degrees(rw_max_rad))
|
||||
|
||||
# --- jerk -> rate limit ---
|
||||
sec2 = 1.2
|
||||
max_drw_dt = (max_lat_jerk * wheelbase_m) / (v * v * sec2) # rad/s
|
||||
max_drw_per_tick = max_drw_dt * DT_CTRL # rad/tick
|
||||
max_drw_per_tick_deg = float(np.degrees(max_drw_per_tick))
|
||||
|
||||
err = abs(target_sw - last_sw_deg)
|
||||
if err > 20.0:
|
||||
max_sw_rate_deg_per_tick = min(max_sw_rate_deg_per_tick, 1.0)
|
||||
|
||||
max_drw_per_tick_deg = min(
|
||||
max_drw_per_tick_deg,
|
||||
max_sw_rate_deg_per_tick / steer_ratio
|
||||
)
|
||||
|
||||
# --- rate limit ---
|
||||
cmd_rw = rate_limit(target_rw, last_rw, -max_drw_per_tick_deg, max_drw_per_tick_deg)
|
||||
|
||||
# --- accel clip ---
|
||||
cmd_rw = float(np.clip(cmd_rw, -rw_max, rw_max))
|
||||
|
||||
if not lat_active:
|
||||
cmd_rw = float(steering_sw_deg) / steer_ratio
|
||||
|
||||
cmd_sw = cmd_rw * steer_ratio
|
||||
return float(np.clip(cmd_sw, -steer_sw_max_deg, steer_sw_max_deg))
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP, CP_IQ=None):
|
||||
super().__init__(dbc_names, CP, CP_IQ or structs.IQCarParams())
|
||||
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.steering_pressed_prev = False
|
||||
self.recovering_from_override = False
|
||||
self.full_recovery_frames = 0
|
||||
self.repeated_override_count = 0
|
||||
self.override_latched = False
|
||||
self.override_release_frames = 0
|
||||
self.driver_torque_filtered = 0.0
|
||||
self.driver_torque_filtered_prev = 0.0
|
||||
self.pre_override_frames = 0
|
||||
|
||||
self.lkas11_active = False
|
||||
|
||||
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.enable_corner_radar = 0
|
||||
|
||||
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, CC_IQ, 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")
|
||||
self.enable_corner_radar = params.get_int("EnableCornerRadar")
|
||||
|
||||
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)
|
||||
|
||||
apply_angle = apply_steer_angle_limits_physics(
|
||||
actuators.steeringAngleDeg,
|
||||
self.apply_angle_last,
|
||||
CS.out.vEgoRaw,
|
||||
CS.out.steeringAngleDeg,
|
||||
CC.latActive,
|
||||
self.CP.wheelbase,
|
||||
self.CP.steerRatio,
|
||||
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
|
||||
CS.modelV2,
|
||||
)
|
||||
|
||||
|
||||
if angle_control:
|
||||
apply_steer_req = CC.latActive
|
||||
|
||||
angle_torque_cap = self.angle_max_torque
|
||||
|
||||
steering_pressed_rising = CS.out.steeringPressed and not self.steering_pressed_prev
|
||||
if steering_pressed_rising:
|
||||
if 0 < self.full_recovery_frames < int(5.0 / DT_CTRL):
|
||||
self.repeated_override_count = min(self.repeated_override_count + 1, 3)
|
||||
self.full_recovery_frames = 0
|
||||
self.recovering_from_override = True
|
||||
|
||||
torque_threshold = max(self.params.STEER_THRESHOLD, 1.0)
|
||||
# Filter signed torque so alternating sensor noise cancels out before its
|
||||
# magnitude is used for pre-override prediction.
|
||||
driver_torque = float(CS.out.steeringTorque)
|
||||
driver_torque_abs = abs(driver_torque)
|
||||
if not CC.latActive:
|
||||
self.driver_torque_filtered = driver_torque
|
||||
self.driver_torque_filtered_prev = driver_torque
|
||||
self.pre_override_frames = 0
|
||||
else:
|
||||
torque_filter_alpha = DT_CTRL / (DRIVER_TORQUE_FILTER_TAU + DT_CTRL)
|
||||
self.driver_torque_filtered_prev = self.driver_torque_filtered
|
||||
self.driver_torque_filtered += torque_filter_alpha * (driver_torque - self.driver_torque_filtered)
|
||||
|
||||
driver_torque_filtered_abs = abs(self.driver_torque_filtered)
|
||||
driver_torque_filtered_prev_abs = abs(self.driver_torque_filtered_prev)
|
||||
driver_torque_rate = max(0.0, (driver_torque_filtered_abs - driver_torque_filtered_prev_abs) / DT_CTRL)
|
||||
torque_ratio = driver_torque_filtered_abs / torque_threshold
|
||||
raw_torque_ratio = driver_torque_abs / torque_threshold
|
||||
predicted_torque_ratio = (
|
||||
driver_torque_filtered_abs + driver_torque_rate * PRE_OVERRIDE_PREDICTION_TIME
|
||||
) / torque_threshold
|
||||
pre_override_candidate = (
|
||||
CC.latActive and
|
||||
not CS.out.steeringPressed and
|
||||
raw_torque_ratio > PRE_OVERRIDE_RAW_MIN_RATIO and
|
||||
torque_ratio > PRE_OVERRIDE_FILTERED_MIN_RATIO and
|
||||
predicted_torque_ratio > PRE_OVERRIDE_START_RATIO and
|
||||
driver_torque_rate > torque_threshold * PRE_OVERRIDE_MIN_RATE_RATIO
|
||||
)
|
||||
self.pre_override_frames = self.pre_override_frames + 1 if pre_override_candidate else 0
|
||||
pre_override_yield = 0.0
|
||||
if self.pre_override_frames >= PRE_OVERRIDE_CONFIRM_FRAMES:
|
||||
pre_override_yield = float(np.interp(
|
||||
predicted_torque_ratio,
|
||||
[PRE_OVERRIDE_START_RATIO, PRE_OVERRIDE_FULL_RATIO],
|
||||
[0.0, 1.0],
|
||||
))
|
||||
recovery_allowed = False
|
||||
|
||||
if CS.out.steeringPressed:
|
||||
# Start yielding immediately when driver override is confirmed.
|
||||
self.override_latched = True
|
||||
self.override_release_frames = 0
|
||||
torque_delta = -20.0
|
||||
elif pre_override_yield > 0.0:
|
||||
# Start handing off gently before steeringPressed flips to avoid a sharp torque drop.
|
||||
torque_delta = PRE_OVERRIDE_MAX_TORQUE_DELTA * pre_override_yield
|
||||
elif self.lkas_max_torque >= self.angle_max_torque:
|
||||
# Once fully recovered, hold full authority until the next driver override.
|
||||
torque_delta = 0.0
|
||||
elif self.override_latched:
|
||||
# Hold reduced authority until driver torque stays below 60% for 0.2 seconds.
|
||||
self.override_release_frames = self.override_release_frames + 1 if torque_ratio < 0.6 else 0
|
||||
if self.override_release_frames >= int(0.2 / DT_CTRL):
|
||||
self.override_latched = False
|
||||
self.override_release_frames = 0
|
||||
recovery_allowed = True
|
||||
else:
|
||||
torque_delta = 0.0
|
||||
else:
|
||||
recovery_allowed = True
|
||||
|
||||
if recovery_allowed:
|
||||
# Use one-second model uncertainty to set the base torque recovery time.
|
||||
# Missing or invalid model data falls back to a moderate 1.5-second recovery.
|
||||
y_std_1s = 0.2
|
||||
if CS.modelV2 is not None and len(CS.modelV2.position.yStd) > 10:
|
||||
model_y_std_1s = float(CS.modelV2.position.yStd[10])
|
||||
if np.isfinite(model_y_std_1s) and model_y_std_1s >= 0.0:
|
||||
y_std_1s = model_y_std_1s
|
||||
|
||||
recovery_time = float(np.interp(y_std_1s, [0.1, 0.2, 0.3, 0.4], [0.5, 0.8, 1.5, 3.0]))
|
||||
recovery_time = max(recovery_time, float(np.interp(
|
||||
self.repeated_override_count,
|
||||
[0, 1, 2, 3],
|
||||
[0.1, 1.0, 2.0, 3.0],
|
||||
)))
|
||||
base_rate_up = (self.angle_max_torque - self.params.ANGLE_MIN_TORQUE) * DT_CTRL / recovery_time
|
||||
|
||||
# During recovery, taper the rate to zero. Only steeringPressed can reduce authority.
|
||||
torque_delta = base_rate_up * float(np.interp(torque_ratio, [0.6, 0.8], [1.0, 0.0]))
|
||||
self.lkas_max_torque = float(np.clip(self.lkas_max_torque + torque_delta,
|
||||
self.params.ANGLE_MIN_TORQUE, angle_torque_cap))
|
||||
|
||||
if not CS.out.steeringPressed and self.recovering_from_override and self.lkas_max_torque >= self.angle_max_torque:
|
||||
self.recovering_from_override = False
|
||||
self.full_recovery_frames = 1
|
||||
elif not CS.out.steeringPressed and self.full_recovery_frames > 0:
|
||||
self.full_recovery_frames += 1
|
||||
if self.full_recovery_frames >= int(5.0 / DT_CTRL):
|
||||
self.full_recovery_frames = 0
|
||||
self.repeated_override_count = 0
|
||||
|
||||
if not CC.latActive:
|
||||
apply_torque = 0
|
||||
self.lkas_max_torque = 0
|
||||
self.recovering_from_override = False
|
||||
self.full_recovery_frames = 0
|
||||
self.repeated_override_count = 0
|
||||
self.override_latched = False
|
||||
self.override_release_frames = 0
|
||||
self.driver_torque_filtered = 0.0
|
||||
self.driver_torque_filtered_prev = 0.0
|
||||
self.pre_override_frames = 0
|
||||
|
||||
self.steering_pressed_prev = CS.out.steeringPressed if CC.latActive else False
|
||||
|
||||
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
|
||||
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 (not hda2 or hda2_long or camera_scc):
|
||||
can_sends.extend(hyundaicanfd.create_lfahda_cluster(self.packer, CS, self.CAN, CC.longActive, CC.latActive))
|
||||
if not camera_scc:
|
||||
can_sends.extend(hyundaicanfd.create_lfa_icon_non_camera_scc(self.packer, CS, self.CAN, CC))
|
||||
|
||||
# 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.enable_corner_radar, stopping, self.canfd_debug))
|
||||
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:
|
||||
msg = 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)
|
||||
if msg is not None:
|
||||
can_sends.append(msg)
|
||||
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:
|
||||
if CS.lkas11 is not None:
|
||||
if self.lkas11_active:
|
||||
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))
|
||||
self.lkas11_active = True
|
||||
|
||||
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"] and CS.mdps12 is not None: # send mdps12 to LKAS to prevent LKAS error
|
||||
can_sends.append(hyundaican.create_mdps12(self.packer, self.frame, CS.mdps12))
|
||||
|
||||
casper_ev = self.CP.carFingerprint == 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_ev, 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_ev:
|
||||
#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
|
||||
# torqueOutputCan reflects the steering authority value actually sent over CAN.
|
||||
# Torque-control platforms send the signed torque command, while angle-control
|
||||
# platforms send LKAS_ANGLE_MAX_TORQUE alongside the requested angle.
|
||||
new_actuators.torqueOutputCan = self.lkas_max_torque if angle_control else 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 CS.clu11 is None:
|
||||
return can_sends
|
||||
|
||||
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.scc_control))
|
||||
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 iqpilot.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:
|
||||
# Keep deceleration authority after the MPC jerk settles to zero. Stock SCC raises the
|
||||
# lower jerk limit with the raw acceleration request instead of relying on jerk alone.
|
||||
jerk_l_base = 1.2
|
||||
jerk_l_raw = np.clip(jerk_l_base + 2.0 * max(0.0, -accel - 2.8), jerk_l_base, jerk_max_l)
|
||||
jerk_l_mpc = np.clip(-self.jerk * 4.0, jerk_l_base, jerk_max_l)
|
||||
|
||||
self.jerk_u = min(max(self.jerk_u_min, self.jerk * 2.0), jerk_max_u)
|
||||
self.jerk_l = max(jerk_l_raw, jerk_l_mpc)
|
||||
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 * 4.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)
|
||||
839
artifacts/package_runtime/iqdbc/car/hyundai/carstate.py
Normal file
839
artifacts/package_runtime/iqdbc/car/hyundai/carstate.py
Normal file
@@ -0,0 +1,839 @@
|
||||
from collections import deque
|
||||
import copy
|
||||
import math
|
||||
import numpy as np
|
||||
import ast
|
||||
|
||||
from iqdbc.can import CANDefine, CANParser
|
||||
from iqdbc.car import Bus, create_button_events, structs, DT_CTRL
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags, CAR, DBC, Buttons, CarControllerParams, CAMERA_SCC_CAR, HyundaiExtFlags, \
|
||||
EV_MODE_ACTIVE_VALUES, EV_MODE_STATUS_ADDR, EV_MODE_STATUS_DLC, EV_MODE_STATUS_MSG, \
|
||||
EV_MODE_STATUS_SIGNAL
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
|
||||
from iqpilot.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
|
||||
|
||||
READY_COUNT_OK = 200
|
||||
TRAILER_DISCONNECT_GRACE_FRAMES = int(5.0 / DT_CTRL)
|
||||
EV_MODE_STATUS_TIMEOUT_NS = 500_000_000
|
||||
|
||||
|
||||
def _get_ev_mode_state(cp: CANParser) -> tuple[bool, bool]:
|
||||
timestamps = cp.ts_nanos.get(EV_MODE_STATUS_MSG)
|
||||
if timestamps is None:
|
||||
return False, False
|
||||
|
||||
timestamp = timestamps.get(EV_MODE_STATUS_SIGNAL, 0)
|
||||
dat = cp.dat.get(EV_MODE_STATUS_ADDR, b"")
|
||||
# The update timestamp advances even when the whole ECAN bus is silent.
|
||||
# last_nonempty_nanos would leave the final decoded state valid forever.
|
||||
age = cp._last_update_nanos - timestamp
|
||||
valid = timestamp > 0 and len(dat) == EV_MODE_STATUS_DLC and not cp.bus_timeout and 0 <= age <= EV_MODE_STATUS_TIMEOUT_NS
|
||||
active = valid and int(cp.vl[EV_MODE_STATUS_MSG][EV_MODE_STATUS_SIGNAL]) in EV_MODE_ACTIVE_VALUES
|
||||
return active, valid
|
||||
|
||||
|
||||
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, CP_IQ=None):
|
||||
super().__init__(CP, CP_IQ or structs.IQCarParams())
|
||||
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"
|
||||
|
||||
self.use_accelerator = self.gear_msg_canfd == "ACCELERATOR"
|
||||
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
|
||||
|
||||
# for generic CAN parsing
|
||||
self.fca11 = None
|
||||
self.scc11 = None
|
||||
self.scc12 = None
|
||||
self.scc13 = None
|
||||
self.scc14 = None
|
||||
self.lkas11 = None
|
||||
self.clu11 = None
|
||||
|
||||
# for CANFD parsing
|
||||
self.scc_control = None
|
||||
self.lfa = None
|
||||
self.lfa_alt = None
|
||||
self.lfahda_cluster = None
|
||||
self.adrv_0x161 = None
|
||||
self.adrv_0x200 = None
|
||||
self.adrv_0x1ea = None
|
||||
self.adrv_0x160 = None
|
||||
self.ccnc_0x162 = None
|
||||
self.hda_info_4a3 = None
|
||||
self.tcs = None
|
||||
self.mdps = None
|
||||
self.steer_touch_2af = None
|
||||
self.cruise_buttons_msg = None
|
||||
self.cam_0x362 = None
|
||||
self.cam_0x2a4 = None
|
||||
self.manual_speed_limit_assist = None
|
||||
self.accelerator = None
|
||||
self.blinkers = None
|
||||
self.blinkers_alt = None
|
||||
self.doors_seatbelts = None
|
||||
self.cruise_buttons_alt2 = 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.op_params = Params()
|
||||
|
||||
self.main_enabled = True if self.op_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")
|
||||
try:
|
||||
fingerprints = ast.literal_eval(fingerprints_str) if fingerprints_str else {i: {} for i in range(8)}
|
||||
except (SyntaxError, ValueError):
|
||||
fingerprints = {i: {} for i in range(8)}
|
||||
#print("fingerprints =", fingerprints)
|
||||
ecu_disabled = False
|
||||
if self.CP.openpilotLongitudinalControl and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC):
|
||||
ecu_disabled = True
|
||||
|
||||
|
||||
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.GEAR = True if 69 in fingerprints[pt_bus] else False
|
||||
self.GEAR_ALT = True if 64 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.cp = None
|
||||
self.cp_cam = None
|
||||
self.cp_alt = None
|
||||
self.controls_ready_count = 0
|
||||
|
||||
# trailer detection
|
||||
self.trailer_connected = False
|
||||
self.trailer_timeout_cnt = 0
|
||||
self.trailer_connected_prev = False
|
||||
self.trailer_status = None
|
||||
|
||||
def monitor_fingerprint(self, can_parsers, canfd):
|
||||
if self.controls_ready_count <= READY_COUNT_OK:
|
||||
if Params().get_bool("ControlsReady"):
|
||||
self.controls_ready_count += 1
|
||||
|
||||
self.cp = can_parsers[Bus.pt]
|
||||
self.cp_cam = can_parsers[Bus.cam]
|
||||
self.cp_alt = can_parsers[Bus.alt] if Bus.alt in can_parsers else None
|
||||
|
||||
def add_if_seen(parser, name, ignore_counter = False):
|
||||
msg = parser.dbc.name_to_msg.get(name)
|
||||
if not msg:
|
||||
print(f"{name} not in DBC")
|
||||
return
|
||||
if msg.address not in parser.seen_addresses:
|
||||
return
|
||||
if msg.address in parser.addresses:
|
||||
return
|
||||
parser._add_message(name, ignore_counter = ignore_counter) # ← 이름으로 등록
|
||||
|
||||
def add_and_cache(parser, name: str, attr: str, ignore_counter: bool = False):
|
||||
add_if_seen(parser, name, ignore_counter)
|
||||
if name in parser.vl: # 등록 성공했을 때만
|
||||
setattr(self, attr, parser.vl[name])
|
||||
return True
|
||||
return False
|
||||
|
||||
if self.controls_ready_count == 50:
|
||||
self.cp.controls_ready = self.cp_cam.controls_ready = True
|
||||
if self.cp_alt is not None:
|
||||
self.cp_alt.controls_ready = True
|
||||
elif self.controls_ready_count == 100:
|
||||
self.cp.enable_capture = self.cp_cam.enable_capture = False
|
||||
if self.cp_alt is not None:
|
||||
self.cp_alt.enable_capture = False
|
||||
elif self.controls_ready_count == 101:
|
||||
print("cp_cam.seen_addresses =", self.cp_cam.seen_addresses)
|
||||
elif self.controls_ready_count == 102:
|
||||
print("cp.seen_addresses =", self.cp.seen_addresses)
|
||||
elif self.controls_ready_count == 103:
|
||||
if self.cp_alt is not None:
|
||||
print("cp_alt.seen_addresses =", self.cp_alt.seen_addresses)
|
||||
else:
|
||||
print("cp_alt.seen_addresses = None")
|
||||
if not canfd:
|
||||
if self.controls_ready_count == 104:
|
||||
cp_cruise = self.cp_cam if self.CP.flags & HyundaiFlags.CAMERA_SCC else self.cp
|
||||
add_and_cache(cp_cruise, "FCA11", "fca11")
|
||||
add_and_cache(self.cp_cam, "LKAS11", "lkas11")
|
||||
add_and_cache(self.cp, "CLU11", "clu11")
|
||||
elif self.controls_ready_count == 105:
|
||||
cp_cruise = self.cp_cam if self.CP.flags & HyundaiFlags.CAMERA_SCC else self.cp
|
||||
scc_messages_expected = not self.CP.openpilotLongitudinalControl or self.CP.flags & HyundaiFlags.CAMERA_SCC
|
||||
if scc_messages_expected:
|
||||
add_and_cache(cp_cruise, "SCC11", "scc11")
|
||||
add_and_cache(cp_cruise, "SCC12", "scc12")
|
||||
add_and_cache(cp_cruise, "SCC13", "scc13")
|
||||
add_and_cache(cp_cruise, "SCC14", "scc14")
|
||||
else: # canfd
|
||||
if self.controls_ready_count == 120:
|
||||
cp_cruise = self.cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else self.cp
|
||||
add_and_cache(cp_cruise, "SCC_CONTROL", "scc_control")
|
||||
elif self.controls_ready_count == 121:
|
||||
add_and_cache(self.cp, "TCS", "tcs")
|
||||
add_and_cache(self.cp, "MDPS", "mdps")
|
||||
add_and_cache(self.cp_cam, "LFA", "lfa")
|
||||
add_and_cache(self.cp_cam, "LFA_ALT", "lfa_alt")
|
||||
add_and_cache(self.cp_cam, "LFAHDA_CLUSTER", "lfahda_cluster")
|
||||
elif self.controls_ready_count == 122:
|
||||
add_and_cache(self.cp_cam, "ADRV_0x161", "adrv_0x161")
|
||||
add_and_cache(self.cp_cam, "ADRV_0x200", "adrv_0x200")
|
||||
add_and_cache(self.cp_cam, "ADRV_0x1ea", "adrv_0x1ea")
|
||||
add_and_cache(self.cp_cam, "ADRV_0x160", "adrv_0x160")
|
||||
add_and_cache(self.cp_cam, "CCNC_0x162", "ccnc_0x162")
|
||||
elif self.controls_ready_count == 123:
|
||||
add_and_cache(self.cp, "HDA_INFO_4A3", "hda_info_4a3")
|
||||
add_and_cache(self.cp, "STEER_TOUCH_2AF", "steer_touch_2af")
|
||||
elif self.controls_ready_count == 124:
|
||||
add_and_cache(self.cp, self.cruise_btns_msg_canfd, "cruise_buttons_msg")
|
||||
if not add_and_cache(self.cp_cam, "CAM_0x362", "cam_0x362") and self.cp_alt is not None:
|
||||
add_and_cache(self.cp_alt, "CAM_0x362", "cam_0x362")
|
||||
if not add_and_cache(self.cp_alt, "CAM_0x2a4", "cam_0x2a4", ignore_counter=True) and self.cp_cam is not None:
|
||||
add_and_cache(self.cp_cam, "CAM_0x2a4", "cam_0x2a4", ignore_counter=True)
|
||||
elif self.controls_ready_count == 125:
|
||||
add_and_cache(self.cp, "MANUAL_SPEED_LIMIT_ASSIST", "manual_speed_limit_assist", ignore_counter = True)
|
||||
if self.gear_msg_canfd == "ACCELERATOR":
|
||||
add_and_cache(self.cp, "ACCELERATOR", "accelerator", ignore_counter = True)
|
||||
add_and_cache(self.cp, "BLINKERS", "blinkers")
|
||||
add_and_cache(self.cp, "BLINKERS_ALT", "blinkers_alt")
|
||||
add_and_cache(self.cp, "DOORS_SEATBELTS", "doors_seatbelts")
|
||||
elif self.controls_ready_count == 126:
|
||||
add_and_cache(self.cp, "CRUISE_BUTTONS_ALT2", "cruise_buttons_alt2", ignore_counter = True)
|
||||
add_and_cache(self.cp, "TRAILER_STATUS", "trailer_status", ignore_counter = True)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
||||
self.monitor_fingerprint(can_parsers, self.CP.flags & HyundaiFlags.CANFD)
|
||||
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.CP.flags & HyundaiFlags.CANFD:
|
||||
return self.update_canfd(can_parsers), self.out_iq
|
||||
|
||||
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
|
||||
|
||||
if cp.ts_nanos["EMS21"]["SCR_UREA_LEVEL"] > 0:
|
||||
ret.ureaGauge = float(np.clip(cp.vl["EMS21"]["SCR_UREA_LEVEL"] / 100.0, 0.0, 1.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 and self.controls_ready_count >= READY_COUNT_OK #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"]
|
||||
if self.CP.carFingerprint == CAR.HYUNDAI_CASPER_EV:
|
||||
ret.gearStep = 0
|
||||
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"]
|
||||
if self.CP.carFingerprint == CAR.KIA_K7:
|
||||
ret.gearStep = cp.vl["LVR11"]["CF_Lvr_GearInf"]
|
||||
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
|
||||
if self.CP.carFingerprint == CAR.HYUNDAI_CASPER_EV and aeb_src == "FCA11":
|
||||
fca_fault = cp_cruise.vl["FCA11"]["FCA_Failinfo"] != 0 or cp_cruise.vl["FCA11"]["FCA_Status"] == 3
|
||||
if fca_fault:
|
||||
aeb_warning = False
|
||||
aeb_braking = False
|
||||
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
|
||||
|
||||
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"]
|
||||
|
||||
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, self.out_iq
|
||||
|
||||
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()
|
||||
|
||||
if self.CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230:
|
||||
ret.evModeActive, ret.evModeValid = _get_ev_mode_state(cp)
|
||||
|
||||
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 if not self.use_accelerator else 0 if self.accelerator is None else self.accelerator["ACCELERATOR_PEDAL"] / offset
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
else:
|
||||
ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"]) if not self.use_accelerator else False if self.accelerator is None else bool(self.accelerator["ACCELERATOR_PEDAL_PRESSED"])
|
||||
|
||||
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
|
||||
#print(cp.vl["TCS"], cp.vl_all["TCS"]["DriverBraking"][-10:])
|
||||
|
||||
if self.doors_seatbelts is not None:
|
||||
ret.doorOpen = self.doors_seatbelts["DRIVER_DOOR"] == 1
|
||||
ret.seatbeltUnlatched = self.doors_seatbelts["DRIVER_SEATBELT"] == 0
|
||||
|
||||
gear = cp.vl[self.gear_msg_canfd]["GEAR"] if not self.use_accelerator else 0 if self.accelerator is None else self.accelerator["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 = all(speed <= STANDSTILL_THRESHOLD for speed in
|
||||
(ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr))
|
||||
|
||||
ret.brakeLights = ret.brakePressed or cp.vl["TCS"]["BrakeLight"] == 1 or ret.aEgo < -0.5
|
||||
|
||||
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
|
||||
|
||||
# steering angle deg값이 이상함. mdps값이 더 신뢰가 가는듯.. torque steering 차량도 확인해야함.
|
||||
#ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
|
||||
#ret.steeringAngleDeg = cp.vl["MDPS"]["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
|
||||
|
||||
trailer_signal = self.trailer_status is not None and self.trailer_status["TRAILER_CONNECTED"] != 0
|
||||
if trailer_signal:
|
||||
# Rising edge is immediate so trailer-specific behavior is preserved.
|
||||
self.trailer_timeout_cnt = 0
|
||||
self.trailer_connected = True
|
||||
elif self.trailer_connected:
|
||||
# During ignition-off, the trailer bit can clear before the cluster shuts down.
|
||||
# Keep suppression active through that transient, while still detecting a real
|
||||
# disconnect after 5 seconds at the 100 Hz CarState update rate.
|
||||
self.trailer_timeout_cnt += 1
|
||||
if self.trailer_timeout_cnt > TRAILER_DISCONNECT_GRACE_FRAMES:
|
||||
self.trailer_connected = False
|
||||
else:
|
||||
self.trailer_timeout_cnt = 0
|
||||
|
||||
ret.trailerConnected = self.trailer_connected
|
||||
|
||||
if self.trailer_connected != self.trailer_connected_prev:
|
||||
print(f"[TRAILER_DEBUG] connected={self.trailer_connected} timeout={self.trailer_timeout_cnt}")
|
||||
self.trailer_connected_prev = self.trailer_connected
|
||||
|
||||
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
|
||||
|
||||
blinkers_info = self.blinkers if self.blinkers is not None else self.blinkers_alt if self.blinkers_alt is not None else None
|
||||
if blinkers_info is not None:
|
||||
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 self.cruise_buttons_alt2 is not None:
|
||||
cruise_button = self.cruise_buttons_alt2["CRUISE_BUTTONS"]
|
||||
else:
|
||||
cruise_button = cp.vl[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"]
|
||||
if cruise_button 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 and self.controls_ready_count >= READY_COUNT_OK #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 or self.main_enabled:
|
||||
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
|
||||
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
|
||||
corner = False
|
||||
corner_infos = [info for info in (self.adrv_0x1ea, self.ccnc_0x162) if info is not None]
|
||||
if corner_infos:
|
||||
def corner_max(signal):
|
||||
return max(info[signal] for info in corner_infos)
|
||||
|
||||
ret.leftLongDist = self.lf_distance = corner_max("LF_DETECT_DISTANCE")
|
||||
ret.rightLongDist = self.rf_distance = corner_max("RF_DETECT_DISTANCE")
|
||||
self.lr_distance = corner_max("LR_DETECT_DISTANCE")
|
||||
self.rr_distance = corner_max("RR_DETECT_DISTANCE")
|
||||
ret.leftLatDist = corner_max("LF_DETECT_LATERAL")
|
||||
ret.rightLatDist = corner_max("RF_DETECT_LATERAL")
|
||||
ret.leftRearLongDist = self.lr_distance
|
||||
ret.rightRearLongDist = self.rr_distance
|
||||
ret.leftRearLatDist = corner_max("LR_DETECT_LATERAL")
|
||||
ret.rightRearLatDist = corner_max("RR_DETECT_LATERAL")
|
||||
corner = True
|
||||
if corner:
|
||||
raw_corner_radar_enabled = (
|
||||
self.op_params.get_int("EnableCornerRadar") > 0 and
|
||||
bool(self.CP.extFlags & (HyundaiExtFlags.CORNER_RADAR_OBJECTS_235.value |
|
||||
HyundaiExtFlags.CORNER_RADAR_OBJECTS_180.value))
|
||||
)
|
||||
left_front_block = (not raw_corner_radar_enabled) and 0 < ret.leftLongDist < 7.0
|
||||
right_front_block = (not raw_corner_radar_enabled) and 0 < ret.rightLongDist < 7.0
|
||||
rear_block_dist = 5.0 if raw_corner_radar_enabled else 7.0
|
||||
left_rear_block = 0 < self.lr_distance < rear_block_dist
|
||||
right_rear_block = 0 < self.rr_distance < rear_block_dist
|
||||
left_block = left_front_block or left_rear_block
|
||||
right_block = right_front_block or right_rear_block
|
||||
if left_block:
|
||||
ret.leftBlindspot = True
|
||||
if right_block:
|
||||
ret.rightBlindspot = True
|
||||
|
||||
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"))
|
||||
|
||||
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
|
||||
|
||||
lane_info = self.cam_0x2a4 if self.cam_0x2a4 is not None else self.cam_0x362
|
||||
|
||||
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"] # 0: none, 1: white, 2: yellow, 3: blue
|
||||
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:
|
||||
if self.manual_speed_limit_assist is not None:
|
||||
#ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
|
||||
ret.cruiseState.nonAdaptive = self.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 self.cruise_buttons_alt2 is not None:
|
||||
if int(self.cruise_buttons_alt2.get("LFA_BTN", 0)) == 1:
|
||||
cruise_button = [Buttons.LFA_BUTTON]
|
||||
else:
|
||||
v = int(self.cruise_buttons_alt2.get("CRUISE_BUTTONS", 0))
|
||||
cruise_button = [v if v < 5 else Buttons.NONE]
|
||||
elif 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"])
|
||||
if self.cruise_buttons_alt2 is not None:
|
||||
self.main_buttons.extend([1 if int(self.cruise_buttons_alt2.get("CRUISE_BUTTONS", 0)) == 8 else 0])
|
||||
else:
|
||||
adaptive_main = cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"]
|
||||
normal_main = cp.vl_all[self.cruise_btns_msg_canfd]["NORMAL_CRUISE_MAIN_BTN"]
|
||||
self.main_buttons.extend(int(adaptive or normal) for adaptive, normal in zip(adaptive_main, normal_main, strict=True))
|
||||
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
|
||||
|
||||
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, CP_IQ=None):
|
||||
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)
|
||||
]
|
||||
|
||||
CAN = CanBus(CP)
|
||||
pt_parser = CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CAN.ECAN)
|
||||
if CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230:
|
||||
# Display-only while the signal is fleet-validated: checksum and freshness
|
||||
# gate the value without making a counter/alive fault disable controls.
|
||||
pt_parser._add_message(EV_MODE_STATUS_MSG, math.nan, ignore_counter=True)
|
||||
|
||||
return {
|
||||
Bus.pt: pt_parser,
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CAN.CAM),
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CAN.ACAN),
|
||||
}
|
||||
|
||||
def get_can_parsers(self, CP, CP_IQ=None):
|
||||
if CP.flags & HyundaiFlags.CANFD:
|
||||
return self.get_can_parsers_canfd(CP)
|
||||
|
||||
return {
|
||||
# EMS21 carries SCR_UREA_LEVEL on diesel platforms. NaN frequency makes
|
||||
# it optional, so gasoline/EV platforms do not fail CAN validity.
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [("EMS21", math.nan), ("TPMS11", math.nan)], 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
|
||||
}
|
||||
1302
artifacts/package_runtime/iqdbc/car/hyundai/fingerprints.py
Normal file
1302
artifacts/package_runtime/iqdbc/car/hyundai/fingerprints.py
Normal file
File diff suppressed because it is too large
Load Diff
403
artifacts/package_runtime/iqdbc/car/hyundai/hyundaican.py
Normal file
403
artifacts/package_runtime/iqdbc/car/hyundai/hyundaican.py
Normal file
@@ -0,0 +1,403 @@
|
||||
import copy
|
||||
import crcmod
|
||||
from iqdbc.car.hyundai.values import CAR, HyundaiFlags
|
||||
|
||||
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
|
||||
|
||||
def suppress_casper_ev_fca11_fault(values):
|
||||
# CASPER EV can report transient FCA faults during camera-SCC handoff.
|
||||
# Keep the copied FCA11 frame non-faulting without changing other cars.
|
||||
fca_fault = values["FCA_Failinfo"] != 0 or values["FCA_Status"] == 3
|
||||
values["FCA_Failinfo"] = 0
|
||||
|
||||
if fca_fault:
|
||||
values["FCA_Status"] = 2
|
||||
values["CF_VSM_Prefill"] = 0
|
||||
values["CF_VSM_HBACmd"] = 0
|
||||
values["CF_VSM_Warn"] = 0
|
||||
values["CF_VSM_BeltCmd"] = 0
|
||||
values["CR_VSM_DecCmd"] = 0
|
||||
values["FCA_CmdAct"] = 0
|
||||
values["FCA_StopReq"] = 0
|
||||
values["CF_VSM_DecCmdAct"] = 0
|
||||
|
||||
return values
|
||||
|
||||
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"] = 0 if CP.carFingerprint in (CAR.KIA_RAY_EV) else 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
|
||||
|
||||
values["CF_Lkas_Chksum"] = 0
|
||||
|
||||
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, suppress_casper_ev_fca, CS, soft_hold_mode):
|
||||
from iqdbc.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 suppress_casper_ev_fca: # CASPER_EV의 경우 FCA11에서 fail이 간헐적 발생함.. 그냥막자.. 원인불명..
|
||||
values = suppress_casper_ev_fca11_fault(copy.copy(CS.fca11))
|
||||
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/iqdbc/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 iqdbc.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/iqdbc/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.copy()
|
||||
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)
|
||||
830
artifacts/package_runtime/iqdbc/car/hyundai/hyundaicanfd.py
Normal file
830
artifacts/package_runtime/iqdbc/car/hyundai/hyundaicanfd.py
Normal file
@@ -0,0 +1,830 @@
|
||||
import copy
|
||||
import numpy as np
|
||||
from iqdbc.car import CanBusBase
|
||||
from iqdbc.car.crc import CRC16_XMODEM
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiExtFlags
|
||||
from iqpilot.common.params import Params
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqpilot.cereal import log
|
||||
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
TurnDirection = log.Desire
|
||||
|
||||
|
||||
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(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": 100, #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": 100,
|
||||
}
|
||||
|
||||
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.cam_0x362 is not None:
|
||||
suppress_msg = "CAM_0x362"
|
||||
lfa_block_msg = CS.cam_0x362
|
||||
elif CS.cam_0x2a4 is not None:
|
||||
suppress_msg = "CAM_0x2a4"
|
||||
lfa_block_msg = CS.cam_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 is not None:
|
||||
values = copy.copy(CS.lfahda_cluster)
|
||||
rx_counter = values.pop("COUNTER", None)
|
||||
else:
|
||||
return []
|
||||
values = {}
|
||||
rx_counter = None
|
||||
values["LFA_OptUsmSta"] = 2
|
||||
values["HDA_OptUsmSta"] = 2
|
||||
values["HDA_CntrlModSta"] = 2 if long_active else 0
|
||||
values["HDA_LFA_SymSta"] = 2 if lat_active else 0
|
||||
return [packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values, rx_counter=rx_counter)]
|
||||
|
||||
def create_lfa_icon_non_camera_scc(packer, CS, CAN, CC):
|
||||
ret = []
|
||||
if CS.adrv_0x161 is not None:
|
||||
values = copy.copy(CS.adrv_0x161)
|
||||
rx_counter = values.pop("COUNTER", None)
|
||||
|
||||
lat_active = CC.latActive
|
||||
lat_enabled = CS.out.latEnabled
|
||||
|
||||
values["LFA_ICON"] = 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
|
||||
|
||||
if values["ALERTS_2"] in [1, 2, 5, 6, 10, 21, 22]:
|
||||
values["ALERTS_2"] = 0
|
||||
values["DAW_ICON"] = 0
|
||||
|
||||
if values["ALERTS_1"] == 0:
|
||||
values["SOUNDS_1"] = 0
|
||||
values["SOUNDS_2"] = 0
|
||||
values["SOUNDS_4"] = 0
|
||||
|
||||
if values["ALERTS_3"] in [3, 4, 11, 12, 13, 14, 17, 19, 26, 7, 8, 9, 10]:
|
||||
values["ALERTS_3"] = 0
|
||||
values["SOUNDS_3"] = 0
|
||||
|
||||
if values["ALERTS_5"] in [1, 2, 3, 4, 5]:
|
||||
values["ALERTS_5"] = 0
|
||||
|
||||
ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values, rx_counter=rx_counter))
|
||||
return ret
|
||||
|
||||
def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, hyundai_jerk, CS):
|
||||
|
||||
if CS.scc_control is None:
|
||||
return None
|
||||
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 = accel #np.clip(accel, accel_last - jn, accel_last + jn)
|
||||
|
||||
values = copy.copy(CS.scc_control)
|
||||
rx_counter = values.pop("COUNTER", None)
|
||||
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
|
||||
|
||||
values["ZEROS_7"] = 1
|
||||
|
||||
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 is not None:
|
||||
values = copy.copy(CS.tcs)
|
||||
#rx_counter = values.pop("COUNTER", None)
|
||||
values["DriverBraking"] = 0
|
||||
values["NEW_SIGNAL_20"] = 0
|
||||
values["NEW_SIGNAL_11"] = 0
|
||||
values["DriverBrakingLowSens"] = 0
|
||||
#values["NEW_SIGNAL_1"] = 0 # accel과 관련.. 옆두부 꺼지는것과 관련? 확인필요
|
||||
#values["ACC_REQ"] = 1 # 옆두부 꺼지는것과 관련? 확인필요.. 항상 켜지게함..
|
||||
values["NEW_SIGNAL_1"] = 0 if values["ACC_REQ"] == 1 else 1 # 옆두부..
|
||||
#ret.append(packer.make_can_msg("TCS", CAN.CAM, values, rx_counter = rx_counter))
|
||||
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)
|
||||
# A held MAIN is reported on this bit and switches some clusters to LIMIT mode.
|
||||
values["NORMAL_CRUISE_MAIN_BTN"] = 0
|
||||
#rx_counter = values.pop("COUNTER", None)
|
||||
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, rx_counter = rx_counter))
|
||||
ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, 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
|
||||
|
||||
|
||||
|
||||
|
||||
def _clip_int(x, lo, hi):
|
||||
return lo if x < lo else hi if x > hi else int(x)
|
||||
|
||||
def _get_desire_and_lane_changing(md):
|
||||
desire = 0
|
||||
lane_changing = 0
|
||||
if md is not None:
|
||||
desire = md.meta.desire.raw
|
||||
ds = md.meta.desireState
|
||||
if len(ds) > 4:
|
||||
if ds[1] > 0.9: lane_changing = 1
|
||||
if ds[2] > 0.9: lane_changing = 2
|
||||
if ds[3] > 0.9: lane_changing = 3
|
||||
if ds[4] > 0.9: lane_changing = 4
|
||||
return desire, lane_changing
|
||||
|
||||
def _apply_lane_desire(values, desire):
|
||||
#values['LANE_CHANGING'] = 0
|
||||
|
||||
if desire == 1: # 좌회전
|
||||
values['LANE_CHANGING'] = 1
|
||||
values["LANELINE_CURVATURE"] = 15
|
||||
values["LANELINE_CURVATURE_DIRECTION"] = 0
|
||||
|
||||
elif desire == 2: # 우회전
|
||||
values['LANE_CHANGING'] = 2
|
||||
values["LANELINE_CURVATURE"] = 15
|
||||
values["LANELINE_CURVATURE_DIRECTION"] = 1
|
||||
|
||||
elif desire == 3: # 좌차선변경
|
||||
values['LANE_CHANGING'] = 3
|
||||
|
||||
elif desire == 4: # 우차선변경
|
||||
values['LANE_CHANGING'] = 4
|
||||
|
||||
def _apply_radar_blink(values, radar_pairs, frame, *,
|
||||
disp_dist=30.0, min_dist=14.0,
|
||||
max_interval=100, t=1.0):
|
||||
"""
|
||||
거리 > min_dist 일 때만 깜빡임.
|
||||
거리 멀수록 interval 커짐(느리게).
|
||||
"""
|
||||
for det_key, dist_key in radar_pairs:
|
||||
dist = values[dist_key]
|
||||
if dist <= min_dist:
|
||||
continue
|
||||
|
||||
d = min(dist, disp_dist)
|
||||
interval = int((1 + (max_interval - 1) * (d / disp_dist)) * t)
|
||||
interval = _clip_int(interval, 1, max_interval)
|
||||
|
||||
blink = (frame // interval) & 1
|
||||
values[det_key] = 2 - blink
|
||||
values[dist_key] = min_dist
|
||||
|
||||
def _suppress_trailer_mode_warning(values, CS):
|
||||
# Logs from IONIQ 9 show ALERTS_5=6 is the periodic
|
||||
# "driver assistance limited in trailer mode" popup.
|
||||
if CS.trailer_connected and values.get("ALERTS_5") == 6:
|
||||
values["ALERTS_5"] = 0
|
||||
|
||||
def _make_ccnc_values(values, CS, lat_active, frame, hud_control,
|
||||
lane_line=True, corner_radar=True,
|
||||
desire=0,
|
||||
blink_pairs=None,
|
||||
blink_t=1.0):
|
||||
if lane_line:
|
||||
curvature = round(CS.out.steeringAngleDeg / 3)
|
||||
mag = min(abs(curvature), 15)
|
||||
curv = mag + (-1 if curvature < 0 else 0)
|
||||
direction = 1 if curvature < 0 else 0
|
||||
values["LANELINE_CURVATURE"] = curv if lat_active else 0
|
||||
values["LANELINE_CURVATURE_DIRECTION"] = direction if lat_active else 0
|
||||
if desire:
|
||||
_apply_lane_desire(values, desire)
|
||||
|
||||
if corner_radar:
|
||||
radar_all = [
|
||||
('LF_DETECT', 'LF_DETECT_DISTANCE'),
|
||||
('RF_DETECT', 'RF_DETECT_DISTANCE'),
|
||||
('LR_DETECT', 'LR_DETECT_DISTANCE'),
|
||||
('RR_DETECT', 'RR_DETECT_DISTANCE'),
|
||||
]
|
||||
for det_key, dist_key in radar_all:
|
||||
if values[det_key] >= 4 and values[dist_key] != 0:
|
||||
values[det_key] = 1
|
||||
|
||||
if blink_pairs:
|
||||
_apply_radar_blink(values, blink_pairs, frame, t=blink_t)
|
||||
|
||||
def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control,
|
||||
disp_angle, left_lane_warning, right_lane_warning,
|
||||
enable_corner_radar, stopping, canfd_debug):
|
||||
ret = []
|
||||
|
||||
md = CS.modelV2
|
||||
if not hasattr(create_ccnc_messages, '_lane_line_check') or frame % 100 == 0:
|
||||
create_ccnc_messages._lane_line_check = Params().get_int("LaneLineCheck")
|
||||
lane_line_check = create_ccnc_messages._lane_line_check
|
||||
desire, lane_changing = _get_desire_and_lane_changing(md)
|
||||
|
||||
if CP.flags & HyundaiFlags.CAMERA_SCC.value:
|
||||
HDA_CntrlModSta = 0
|
||||
HDA_LFA_SymSta = 0
|
||||
if CS.lfahda_cluster is not None:
|
||||
HDA_CntrlModSta = CS.lfahda_cluster["HDA_CntrlModSta"]
|
||||
HDA_LFA_SymSta = CS.lfahda_cluster["HDA_LFA_SymSta"]
|
||||
|
||||
if frame % 2 == 0:
|
||||
#if CS.adrv_0x160 is not None:
|
||||
# values = copy.copy(CS.adrv_0x160)
|
||||
# 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)
|
||||
# Keep the physical long press on ECAN for CarState, but don't forward it to CAM.
|
||||
values["NORMAL_CRUISE_MAIN_BTN"] = 0
|
||||
|
||||
if HDA_LFA_SymSta == 0 and 0 < frame % 200 < 12:
|
||||
values["LFA_BTN"] = 1
|
||||
|
||||
if CC.enabled:
|
||||
if not CS.MainMode_ACC:
|
||||
if 10 < frame % 200 <= 16 and CS.out.vEgo > 3.:
|
||||
values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
|
||||
elif CS.ACCMode in [0, 4]:
|
||||
if 10 < frame % 200 <= 16 and CS.out.vEgo > 3.:
|
||||
values["CRUISE_BUTTONS"] = 2
|
||||
elif CS.scc_control is not None and CS.scc_control["InfoDisplay"] == 4:
|
||||
if 10 < frame % 30 <= 16 and not stopping:
|
||||
values["CRUISE_BUTTONS"] = 2
|
||||
else:
|
||||
if CS.adrv_0x1ea is not None and CS.adrv_0x1ea["HDA_MODE2"] == 0: # if corner radar is disabled, send main btn
|
||||
if 10 < frame % 1000 <= 16 and CS.out.vEgo > 3:
|
||||
values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
|
||||
|
||||
ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
|
||||
|
||||
# --- 0x161/0x200/0x1ea/0x162 (frame%5) ---
|
||||
if frame % 5 == 0:
|
||||
lat_active = CC.latActive
|
||||
|
||||
if CS.adrv_0x161 is not None:
|
||||
main_enabled = CS.out.cruiseState.available
|
||||
cruise_enabled = CC.enabled
|
||||
lat_enabled = CS.out.latEnabled
|
||||
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_0x161)
|
||||
rx_counter = values.pop("COUNTER", None)
|
||||
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 hud_control.leadVisible and cruise_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 lat_active else 7
|
||||
values["CENTERLINE"] = 1 if HDA_CntrlModSta > 0 else 0
|
||||
values["CAR_CIRCLE"] = 2 if hdp_active else 1 if cruise_enabled else 0
|
||||
|
||||
values["NAV_ICON"] = 2 if nav_active and cruise_enabled else 1 if main_enabled and 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, 6, 10, 21, 22]:
|
||||
values["ALERTS_2"] = 0
|
||||
values["DAW_ICON"] = 0
|
||||
|
||||
if values["ALERTS_1"] == 0: # alerts가 있으면 사운드도 같이 나옴
|
||||
values["SOUNDS_1"] = 0
|
||||
values["SOUNDS_2"] = 0
|
||||
values["SOUNDS_4"] = 0
|
||||
|
||||
if values["ALERTS_3"] in [3, 4, 11, 12, 13, 14, 17, 19, 20, 26, 27, 28, 7, 8, 9, 10]: # hide gap distance msg.(11,12,13,14), lanechange(19,20,27, 28)
|
||||
values["ALERTS_3"] = 0
|
||||
values["SOUNDS_3"] = 0
|
||||
|
||||
if values["ALERTS_5"] in [1, 2, 3, 4, 5]:
|
||||
values["ALERTS_5"] = 0
|
||||
|
||||
if values["ALERTS_5"] in [11] and CS.softHoldActive == 0:
|
||||
values["ALERTS_5"] = 0
|
||||
|
||||
# curvature 표시(0x161쪽 기존 로직 유지)
|
||||
_suppress_trailer_mode_warning(values, CS)
|
||||
|
||||
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
|
||||
|
||||
trailer_lane_change_blocked = CS.trailer_connected
|
||||
if trailer_lane_change_blocked:
|
||||
values["LANELINE_LEFT"] = 2 if hud_control.leftLaneVisible else 0
|
||||
values["LANELINE_RIGHT"] = 2 if hud_control.rightLaneVisible else 0
|
||||
else:
|
||||
lane_color = 6 if md is not None and md.meta.laneChangeAvailableLeft else 2
|
||||
if lane_line_check >= 1:
|
||||
lane_line_warn_left = CS.out.leftLaneLine % 10 not in (0, 5)
|
||||
else:
|
||||
lane_line_warn_left = CS.out.leftLaneLine // 10 == 2
|
||||
lane_color = 4 if lane_line_warn_left or CS.out.leftBlindspot else lane_color
|
||||
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
|
||||
|
||||
lane_color = 6 if md is not None and md.meta.laneChangeAvailableRight else 2
|
||||
if lane_line_check >= 1:
|
||||
lane_line_warn_right = CS.out.rightLaneLine % 10 not in (0, 5)
|
||||
else:
|
||||
lane_line_warn_right = CS.out.rightLaneLine // 10 == 2
|
||||
lane_color = 4 if lane_line_warn_right or CS.out.rightBlindspot else lane_color
|
||||
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["LCA_LEFT_ARROW"] = 2 if CS.out.leftBlinker else 0
|
||||
values["LCA_RIGHT_ARROW"] = 2 if CS.out.rightBlinker else 0
|
||||
|
||||
if trailer_lane_change_blocked:
|
||||
values["LCA_LEFT_ICON"] = 1 if lat_active else 0
|
||||
values["LCA_RIGHT_ICON"] = 1 if lat_active else 0
|
||||
else:
|
||||
values["LCA_LEFT_ICON"] = (1 if CS.out.leftBlindspot else 2) if lat_active else 0
|
||||
values["LCA_RIGHT_ICON"] = (1 if CS.out.rightBlindspot else 2) if lat_active else 0
|
||||
|
||||
values["LANE_LEFT"] = 0 if trailer_lane_change_blocked else 1 if desire in (1, 3) else 0
|
||||
values["LANE_RIGHT"] = 0 if trailer_lane_change_blocked else 1 if desire in (2, 4) else 0
|
||||
|
||||
ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values, rx_counter = rx_counter))
|
||||
|
||||
if CS.adrv_0x200 is not None:
|
||||
values = copy.copy(CS.adrv_0x200)
|
||||
rx_counter = values.pop("COUNTER", None)
|
||||
values["TauGapSet"] = hud_control.leadDistanceBars
|
||||
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values, rx_counter = rx_counter))
|
||||
|
||||
if CS.adrv_0x1ea is not None:
|
||||
values = copy.copy(CS.adrv_0x1ea)
|
||||
rx_counter = values.pop("COUNTER", None)
|
||||
# blinker hold
|
||||
values['LEFT_BLINK_HOLD'] = 1 if lane_changing == 3 else 0
|
||||
values['RIGHT_BLINK_HOLD'] = 1 if lane_changing == 4 else 0
|
||||
|
||||
_make_ccnc_values(
|
||||
values, CS, lat_active, frame, hud_control,
|
||||
lane_line=True,
|
||||
corner_radar=True,
|
||||
desire=desire,
|
||||
# 기존대로 LR/RR만 깜빡임
|
||||
blink_pairs=[('LR_DETECT', 'LR_DETECT_DISTANCE'),
|
||||
('RR_DETECT', 'RR_DETECT_DISTANCE')],
|
||||
blink_t=1.0
|
||||
)
|
||||
|
||||
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values, rx_counter = rx_counter))
|
||||
|
||||
if CS.ccnc_0x162 is not None:
|
||||
values = copy.copy(CS.ccnc_0x162)
|
||||
|
||||
if hud_control.leadDistance > 0:
|
||||
values["FF_DISTANCE"] = hud_control.leadDistance
|
||||
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
|
||||
|
||||
_make_ccnc_values(
|
||||
values, CS, lat_active, frame, hud_control,
|
||||
lane_line=False,
|
||||
corner_radar=True,
|
||||
desire=0,
|
||||
# 필요하면 162도 깜빡임 적용(원래 코드처럼 LR/RR만)
|
||||
blink_pairs=[('LR_DETECT', 'LR_DETECT_DISTANCE'),
|
||||
('RR_DETECT', 'RR_DETECT_DISTANCE')],
|
||||
blink_t=1.0
|
||||
)
|
||||
|
||||
if (left_lane_warning and not CS.out.leftBlinker) or (right_lane_warning and not CS.out.rightBlinker):
|
||||
values["VIBRATE"] = 1
|
||||
|
||||
if canfd_debug > 0:
|
||||
values["FAULT_LSS"] = 0
|
||||
values["FAULT_DAS"] = 0
|
||||
|
||||
ret.append(packer.make_can_msg("CCNC_0x162", CAN.ECAN, values))
|
||||
|
||||
# --- NEW_MSG_4B9 (corner radar keep-alive?) ---
|
||||
if enable_corner_radar > 0:
|
||||
if HDA_CntrlModSta == 0:
|
||||
if frame % 500 in [10, 20, 30]:
|
||||
values = {
|
||||
'BYTE_1': 0,
|
||||
'BYTE_2': 0,
|
||||
'BYTE_3': 0x80,
|
||||
'BYTE_4': 0x8A,
|
||||
'BYTE_5': 0x32,
|
||||
'BYTE_6': 0x30,
|
||||
'BYTE_7': 0x01,
|
||||
'BYTE_8': 0x00,
|
||||
}
|
||||
ret.append(packer.make_can_msg("NEW_MSG_4B9", CAN.CAM, values))
|
||||
elif frame % 500 in [40, 50, 60]:
|
||||
values = {
|
||||
'BYTE_1': 0xff,
|
||||
'BYTE_2': 0xff,
|
||||
'BYTE_3': 0xff,
|
||||
'BYTE_4': 0xff,
|
||||
'BYTE_5': 0xff,
|
||||
'BYTE_6': 0xff,
|
||||
'BYTE_7': 0xff,
|
||||
'BYTE_8': 0xff,
|
||||
}
|
||||
ret.append(packer.make_can_msg("NEW_MSG_4B9", CAN.CAM, values))
|
||||
|
||||
if False: # canfd_debug > 1 and frame % 20 == 0:
|
||||
if CS.hda_info_4a3 is not None:
|
||||
values = copy.copy(CS.hda_info_4a3)
|
||||
values["LinkClass"] = 1
|
||||
values["SPEED_LIMIT"] = 100
|
||||
ret.append(packer.make_can_msg("HDA_INFO_4A3", CAN.CAM, values))
|
||||
|
||||
return ret
|
||||
315
artifacts/package_runtime/iqdbc/car/hyundai/interface.py
Normal file
315
artifacts/package_runtime/iqdbc/car/hyundai/interface.py
Normal file
@@ -0,0 +1,315 @@
|
||||
from iqdbc.car import Bus, get_safety_config, structs
|
||||
from iqdbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ, CAR, DBC, CANFD_RADAR_SCC_CAR, \
|
||||
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, \
|
||||
UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, HyundaiSafetyFlagsIQ, HyundaiExtFlags, \
|
||||
CANFD_HYBRID_STATUS_ADDR, CANFD_HYBRID_STATUS_DLC, \
|
||||
EV_MODE_STATUS_ADDR, EV_MODE_STATUS_DLC
|
||||
from iqdbc.car.hyundai.radar_interface import RADAR_START_ADDR
|
||||
from iqdbc.car.interfaces import CarInterfaceBase
|
||||
from iqdbc.car.disable_ecu import disable_ecu
|
||||
from iqdbc.car.hyundai.carcontroller import CarController
|
||||
from iqdbc.car.hyundai.carstate import CarState
|
||||
from iqdbc.car.hyundai.radar_interface import RadarInterface
|
||||
|
||||
from iqpilot.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"
|
||||
|
||||
if candidate == CAR.KIA_SORENTO:
|
||||
ret.extFlags |= HyundaiExtFlags.RADAR_GROUP4.value
|
||||
|
||||
cam_can = CanBus(None, fingerprint).CAM if camera_scc == 0 else 1
|
||||
hda2 = 0x50 in fingerprint[cam_can] or 0x110 in fingerprint[cam_can] or params.get_int("CanfdHDA2") > 0
|
||||
CAN = CanBus(None, fingerprint, hda2)
|
||||
|
||||
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
|
||||
|
||||
# Keep drivetrain/safety classification unchanged: this capability is display-only and requires both exact ECAN frames.
|
||||
has_ev_mode_status = fingerprint[CAN.ECAN].get(CANFD_HYBRID_STATUS_ADDR) == CANFD_HYBRID_STATUS_DLC and \
|
||||
fingerprint[CAN.ECAN].get(EV_MODE_STATUS_ADDR) == EV_MODE_STATUS_DLC
|
||||
if has_ev_mode_status:
|
||||
ret.extFlags |= HyundaiExtFlags.EV_MODE_STATUS_230.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
|
||||
elif 0x400 in fingerprint[CAN.ACAN] and 0x41D in fingerprint[CAN.ACAN]:
|
||||
print("##### Radar Group 3 detected (0x400-0x41D)")
|
||||
ret.extFlags |= HyundaiExtFlags.RADAR_GROUP3.value
|
||||
if all(fingerprint[CAN.ACAN].get(addr) == 32 for addr in range(0x235, 0x249)):
|
||||
ret.extFlags |= HyundaiExtFlags.CORNER_RADAR_OBJECTS_235.value
|
||||
print("##### Corner radar objects 0x235 group detected")
|
||||
if all(fingerprint[CAN.ACAN].get(addr) == 32 for addr in range(0x180, 0x185)):
|
||||
ret.extFlags |= HyundaiExtFlags.CORNER_RADAR_OBJECTS_180.value
|
||||
print("##### Corner radar objects 0x180 group detected")
|
||||
if all(fingerprint[CAN.ACAN].get(addr) == 32 for addr in tuple(range(0x430, 0x438)) + tuple(range(0x440, 0x448))):
|
||||
ret.extFlags |= HyundaiExtFlags.CORNER_RADAR_OBJECTS_430.value
|
||||
print("##### Corner radar objects 0x430/0x440 group detected")
|
||||
|
||||
# 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
|
||||
#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
|
||||
enable_radar_tracks = params.get_int("EnableRadarTracks")
|
||||
if ret.flags & HyundaiFlags.CAMERA_SCC.value or enable_radar_tracks > 0 or enable_radar_tracks == -2:
|
||||
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{enable_radar_tracks}")
|
||||
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 _get_params_iq(stock_cp, ret, candidate, fingerprint, car_fw, alpha_long, is_release_iq, docs):
|
||||
del candidate, car_fw, alpha_long, is_release_iq, docs
|
||||
if not stock_cp.flags & HyundaiFlags.CANFD and 0x391 in fingerprint[0]:
|
||||
ret.flags |= HyundaiFlagsIQ.HAS_LFA_BUTTON
|
||||
ret.iqSafetyFlags |= HyundaiSafetyFlagsIQ.HAS_LDA_BUTTON
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def init(CP, CP_IQ, can_recv, can_send):
|
||||
del CP_IQ
|
||||
|
||||
Params().put_int('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 iqdbc.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
|
||||
885
artifacts/package_runtime/iqdbc/car/hyundai/radar_interface.py
Normal file
885
artifacts/package_runtime/iqdbc/car/hyundai/radar_interface.py
Normal file
@@ -0,0 +1,885 @@
|
||||
import math
|
||||
import os
|
||||
from collections import deque
|
||||
|
||||
from iqdbc import DBC_PATH
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.interfaces import RadarInterfaceBase
|
||||
from iqdbc.car.hyundai.values import DBC, HyundaiFlags, HyundaiExtFlags
|
||||
from iqpilot.common.params import Params
|
||||
from iqdbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from iqpilot.common.filter_simple import MyMovingAverage
|
||||
|
||||
SCC_TID = 0
|
||||
RADAR_START_ADDR = 0x500
|
||||
RADAR_MSG_COUNT = 32
|
||||
RADAR_MSG_COUNT4 = 8
|
||||
RADAR_GROUP4_MAX_LONG_DIST = 325.0
|
||||
RADAR_GROUP4_MAX_YREL = 6.0
|
||||
RADAR_START_ADDR_CANFD1 = 0x210
|
||||
RADAR_MSG_COUNT1 = 16
|
||||
RADAR_START_ADDR_CANFD2 = 0x3A5 # Group 2, Group 1: 0x210 2개씩?어???단 보류.
|
||||
RADAR_MSG_COUNT2 = 32
|
||||
RADAR_START_ADDR_CANFD3 = 0x400
|
||||
RADAR_MSG_COUNT3 = 30
|
||||
CORNER_OBJECT_235_START_ADDR = 0x235
|
||||
CORNER_OBJECT_235_MSG_COUNT = 20
|
||||
CORNER_OBJECT_235_TRACK_ID_OFFSET = 200
|
||||
CORNER_OBJECT_235_DBC = 'hyundai_canfd_corner_radar_235_generated'
|
||||
CORNER_OBJECT_180_START_ADDR = 0x180
|
||||
CORNER_OBJECT_180_MSG_COUNT = 5
|
||||
CORNER_OBJECT_180_SLOTS_PER_MSG = 2
|
||||
CORNER_OBJECT_180_TRACK_ID_OFFSET = 240
|
||||
CORNER_OBJECT_180_DBC = 'hyundai_canfd_corner_radar_180_generated'
|
||||
CORNER_OBJECT_430_LEFT_START_ADDR = 0x430
|
||||
CORNER_OBJECT_430_RIGHT_START_ADDR = 0x440
|
||||
CORNER_OBJECT_430_MSG_COUNT_PER_SIDE = 8
|
||||
CORNER_OBJECT_430_SLOTS_PER_MSG = 7
|
||||
CORNER_OBJECT_430_TRACK_ID_OFFSET = 300
|
||||
CORNER_OBJECT_430_DBC = 'hyundai_canfd_corner_radar_430_generated'
|
||||
CORNER_OBJECT_430_EMPTY_RAW_VALUES = (0x010d1f40, 0x00010d1f)
|
||||
CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MIN = 2520 # 126.0 m
|
||||
CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MAX = 2600 # 130.0 m
|
||||
CORNER_OBJECT_430_MAX_DREL = 120.0
|
||||
CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE = 4
|
||||
CORNER_OBJECT_430_DT = 0.05
|
||||
CORNER_OBJECT_430_MAX_DREL_DELTA = 1.5
|
||||
CORNER_OBJECT_430_CANDIDATE_META_BYTE_3 = (2,)
|
||||
CORNER_OBJECT_430_CANDIDATE_EXCLUDED_SLOTS = (1,)
|
||||
CORNER_OBJECT_430_CANDIDATE_RAW_DELTA = 200
|
||||
CORNER_OBJECT_430_STRONG_META_BYTE_2 = (10,)
|
||||
CORNER_OBJECT_430_WEAK_META_BYTE_2 = (5, 6, 7, 8, 9)
|
||||
CORNER_OBJECT_430_STRONG_MIN_SUPPORT = 2
|
||||
CORNER_OBJECT_430_WEAK_MIN_SUPPORT = 3
|
||||
CORNER_OBJECT_430_CLUSTER_RAW_GAP = 200
|
||||
CORNER_OBJECT_430_TRACK_MATCH_MAX_DREL_DELTA = 3.0
|
||||
CORNER_OBJECT_430_MAX_ABS_VREL = 20.0
|
||||
CORNER_OBJECT_430_MAX_ABS_YVREL = 3.0
|
||||
CORNER_OBJECT_430_VREL_ALPHA = 0.35
|
||||
CORNER_OBJECT_430_YVREL_ALPHA = 0.35
|
||||
CORNER_OBJECT_430_LATERAL_CELL_MSG_WEIGHT = 0.35
|
||||
CORNER_OBJECT_430_LATERAL_CELL_SLOT_WEIGHT = 0.65
|
||||
CORNER_OBJECT_430_YREL_OFFSET = 5.8
|
||||
CORNER_OBJECT_430_YREL_SCALE = 1.1
|
||||
CORNER_OBJECT_430_RIGHT_CELL_MIRROR = 7.0
|
||||
CORNER_OBJECT_430_MIN_ABS_YREL = 0.8
|
||||
CORNER_OBJECT_430_MAX_ABS_YREL = 4.2
|
||||
CORNER_OBJECT_430_HISTORY_SIZE = 8
|
||||
CORNER_OBJECT_430_MIN_HISTORY = 5
|
||||
CORNER_OBJECT_430_MIN_INWARD_YREL_DELTA = 0.35
|
||||
CORNER_OBJECT_430_MIN_RECENT_INWARD_YREL_DELTA = 0.05
|
||||
CORNER_OBJECT_430_MIN_INWARD_RATIO = 0.65
|
||||
CORNER_OBJECT_430_INWARD_CENTER_ABS_YREL = 1.55
|
||||
CORNER_OBJECT_430_INWARD_KEEP_YVREL_ABS_YREL = 2.2
|
||||
CORNER_OBJECT_430_EARLY_INWARD_NONCENTER_FRAMES = 2
|
||||
CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL = 2.0
|
||||
CORNER_OBJECT_STABLE_TRACK_ID_START = 1000
|
||||
CORNER_SIDE_OBJECT_MAX_DREL = 0.2
|
||||
CORNER_SIDE_OBJECT_MIN_ABS_YREL = 1.4
|
||||
CORNER_SIDE_OBJECT_MAX_ABS_YREL = 4.5
|
||||
|
||||
# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
|
||||
|
||||
|
||||
class CornerObjectTrackIdManager:
|
||||
def __init__(self):
|
||||
self.next_track_id = CORNER_OBJECT_STABLE_TRACK_ID_START
|
||||
self.objects: dict[tuple[str, int], tuple[int, int]] = {}
|
||||
|
||||
def get_track_id(self, source: str, object_id: int, age: int) -> int:
|
||||
key = (source, object_id)
|
||||
previous = self.objects.get(key)
|
||||
if previous is None or age < previous[1]:
|
||||
track_id = self.next_track_id
|
||||
self.next_track_id += 1
|
||||
else:
|
||||
track_id = previous[0]
|
||||
self.objects[key] = (track_id, age)
|
||||
return track_id
|
||||
|
||||
def clear_source(self, source: str):
|
||||
self.objects = {key: value for key, value in self.objects.items() if key[0] != source}
|
||||
|
||||
|
||||
def corner_object_position_valid(d_rel: float, y_rel: float) -> bool:
|
||||
normal_object = 0.2 < d_rel < 180.0
|
||||
clipped_side_object = (
|
||||
0.0 <= d_rel <= CORNER_SIDE_OBJECT_MAX_DREL and
|
||||
CORNER_SIDE_OBJECT_MIN_ABS_YREL <= abs(y_rel) <= CORNER_SIDE_OBJECT_MAX_ABS_YREL
|
||||
)
|
||||
return (normal_object or clipped_side_object) and abs(y_rel) < 40.0
|
||||
|
||||
|
||||
def get_radar_can_parser(CP, radar_tracks, msg_start_addr, msg_count, radar_group4=False):
|
||||
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)
|
||||
dbc_name = 'hyundai_kia_denso_front_radar_generated' if radar_group4 else 'hyundai_kia_mando_front_radar_generated'
|
||||
return CANParser(dbc_name, messages, 1)
|
||||
|
||||
def get_corner_object_can_parser(CP, enabled):
|
||||
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
|
||||
return None
|
||||
|
||||
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_235_DBC}.dbc")
|
||||
if not os.path.exists(dbc_path):
|
||||
print(f"RadarInterface: missing {CORNER_OBJECT_235_DBC}.dbc, 0x235 corner radar disabled")
|
||||
return None
|
||||
|
||||
CAN = CanBus(CP)
|
||||
messages = [(f"CORNER_RADAR_235_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_235_START_ADDR, CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT)]
|
||||
return CANParser(CORNER_OBJECT_235_DBC, messages, CAN.ACAN)
|
||||
|
||||
def get_corner_object_180_can_parser(CP, enabled):
|
||||
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
|
||||
return None
|
||||
|
||||
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_180_DBC}.dbc")
|
||||
if not os.path.exists(dbc_path):
|
||||
print(f"RadarInterface: missing {CORNER_OBJECT_180_DBC}.dbc, 0x180 corner radar disabled")
|
||||
return None
|
||||
|
||||
CAN = CanBus(CP)
|
||||
messages = [(f"CORNER_RADAR_180_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_180_START_ADDR, CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT)]
|
||||
return CANParser(CORNER_OBJECT_180_DBC, messages, CAN.ACAN)
|
||||
|
||||
def get_corner_object_430_can_parser(CP, enabled):
|
||||
if not enabled or not (CP.flags & HyundaiFlags.CANFD):
|
||||
return None
|
||||
|
||||
dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_430_DBC}.dbc")
|
||||
if not os.path.exists(dbc_path):
|
||||
print(f"RadarInterface: missing {CORNER_OBJECT_430_DBC}.dbc, 0x430/0x440 corner radar disabled")
|
||||
return None
|
||||
|
||||
CAN = CanBus(CP)
|
||||
messages = [(f"CORNER_RADAR_430_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_430_LEFT_START_ADDR, CORNER_OBJECT_430_LEFT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)]
|
||||
messages += [(f"CORNER_RADAR_430_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_430_RIGHT_START_ADDR, CORNER_OBJECT_430_RIGHT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)]
|
||||
return CANParser(CORNER_OBJECT_430_DBC, messages, CAN.ACAN)
|
||||
|
||||
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, CP_IQ=None):
|
||||
super().__init__(CP, CP_IQ or structs.IQCarParams())
|
||||
self.v_ego = 0.0
|
||||
|
||||
self.canfd = True if CP.flags & HyundaiFlags.CANFD else False
|
||||
self.radar_group1 = False
|
||||
self.radar_group3 = False
|
||||
self.radar_group4 = not self.canfd and bool(CP.extFlags & HyundaiExtFlags.RADAR_GROUP4.value)
|
||||
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
|
||||
elif CP.extFlags & HyundaiExtFlags.RADAR_GROUP3.value:
|
||||
self.radar_start_addr = RADAR_START_ADDR_CANFD3
|
||||
self.radar_msg_count = RADAR_MSG_COUNT3
|
||||
self.radar_group3 = 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_COUNT4 if self.radar_group4 else RADAR_MSG_COUNT
|
||||
|
||||
self.params = Params()
|
||||
self.radar_tracks = self.params.get_int("EnableRadarTracks") >= 1
|
||||
self.corner_object_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_235.value) and self.params.get_int("EnableCornerRadar") > 0
|
||||
self.corner_object_180_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_180.value) and self.params.get_int("EnableCornerRadar") > 0
|
||||
self.corner_object_430_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_430.value) and self.params.get_int("EnableCornerRadar") > 0
|
||||
self.updated_tracks = set()
|
||||
self.updated_scc = set()
|
||||
self.updated_corner_objects = set()
|
||||
self.updated_corner_objects_180 = set()
|
||||
self.updated_corner_objects_430 = set()
|
||||
self.corner_object_missed_updates = 0
|
||||
self.corner_object_180_missed_updates = 0
|
||||
self.corner_object_430_missed_updates = 0
|
||||
self.corner_object_track_ids = CornerObjectTrackIdManager()
|
||||
self.rcp_tracks = get_radar_can_parser(CP, self.radar_tracks, self.radar_start_addr, self.radar_msg_count, self.radar_group4)
|
||||
self.rcp_corner_objects = get_corner_object_can_parser(CP, self.corner_object_tracks)
|
||||
self.rcp_corner_objects_180 = get_corner_object_180_can_parser(CP, self.corner_object_180_tracks)
|
||||
self.rcp_corner_objects_430 = get_corner_object_430_can_parser(CP, self.corner_object_430_tracks)
|
||||
# Enabling raw radar tracks on legacy CAN disables the stock SCC11 stream on
|
||||
# some Hyundai/Kia platforms. Camera-SCC cars may still use SCC11.
|
||||
use_scc_parser = not (self.radar_tracks and not self.canfd and not (CP.flags & HyundaiFlags.CAMERA_SCC))
|
||||
self.rcp_scc = get_radar_can_parser_scc(CP) if use_scc_parser else None
|
||||
self.trigger_msg_scc = 416 if self.canfd else 0x420
|
||||
|
||||
self.trigger_msg_tracks = self.radar_start_addr + self.radar_msg_count - 1
|
||||
self.trigger_msg_corner_objects = CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT - 1
|
||||
self.trigger_msg_corner_objects_180 = CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT - 1
|
||||
self.trigger_msg_corner_objects_430 = CORNER_OBJECT_430_RIGHT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE - 1
|
||||
self.track_id = 0
|
||||
|
||||
self.corner_objects_available = self.rcp_corner_objects is not None or self.rcp_corner_objects_180 is not None or self.rcp_corner_objects_430 is not None
|
||||
self.radar_off_can = CP.radarUnavailable and not self.corner_objects_available
|
||||
print(
|
||||
"RadarInterface: "
|
||||
f"radarUnavailable={CP.radarUnavailable} radarTracks={self.radar_tracks} "
|
||||
f"group4={self.radar_group4} "
|
||||
f"corner235={self.rcp_corner_objects is not None} corner180={self.rcp_corner_objects_180 is not None} "
|
||||
f"corner430={self.rcp_corner_objects_430 is not None} "
|
||||
f"radarOffCan={self.radar_off_can}"
|
||||
)
|
||||
|
||||
self.vRel_last = 0
|
||||
self.dRel_last = 0
|
||||
self.corner_object_430_prev_d_rel = {}
|
||||
self.corner_object_430_prev_v_rel = {}
|
||||
self.corner_object_430_prev_y_rel = {}
|
||||
self.corner_object_430_prev_yv_rel = {}
|
||||
self.corner_object_430_prev_code = {}
|
||||
self.corner_object_430_history = {}
|
||||
self.corner_object_430_noncenter_inward_frames = {}
|
||||
|
||||
# Initialize pts
|
||||
if self.rcp_tracks is not None:
|
||||
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
|
||||
|
||||
if self.rcp_scc is not None:
|
||||
self.pts[SCC_TID] = structs.RadarData.RadarPoint()
|
||||
self.pts[SCC_TID].trackId = SCC_TID
|
||||
self.pts[SCC_TID].radarSource = "scc"
|
||||
if self.rcp_corner_objects is not None:
|
||||
for slot in range(CORNER_OBJECT_235_MSG_COUNT):
|
||||
t_id = CORNER_OBJECT_235_TRACK_ID_OFFSET + slot
|
||||
self.pts[t_id] = structs.RadarData.RadarPoint()
|
||||
self.pts[t_id].measured = False
|
||||
self.pts[t_id].trackId = t_id
|
||||
self.pts[t_id].radarSource = "corner235"
|
||||
if self.rcp_corner_objects_180 is not None:
|
||||
for slot in range(CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG):
|
||||
t_id = CORNER_OBJECT_180_TRACK_ID_OFFSET + slot
|
||||
self.pts[t_id] = structs.RadarData.RadarPoint()
|
||||
self.pts[t_id].measured = False
|
||||
self.pts[t_id].trackId = t_id
|
||||
self.pts[t_id].radarSource = "corner180"
|
||||
if self.rcp_corner_objects_430 is not None:
|
||||
for slot in range(CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * 2 * CORNER_OBJECT_430_SLOTS_PER_MSG):
|
||||
t_id = CORNER_OBJECT_430_TRACK_ID_OFFSET + slot
|
||||
self.pts[t_id] = structs.RadarData.RadarPoint()
|
||||
self.pts[t_id].measured = False
|
||||
self.pts[t_id].trackId = t_id
|
||||
|
||||
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 and self.rcp_corner_objects is None and self.rcp_corner_objects_180 is None and self.rcp_corner_objects_430 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)
|
||||
|
||||
track_ready = False
|
||||
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)
|
||||
track_ready = self.trigger_msg_tracks in self.updated_tracks
|
||||
|
||||
corner_ready = False
|
||||
if self.rcp_corner_objects is not None:
|
||||
vls_c = self.rcp_corner_objects.update(can_strings)
|
||||
self.updated_corner_objects.update(vls_c)
|
||||
corner_ready = self.trigger_msg_corner_objects in self.updated_corner_objects
|
||||
|
||||
corner_180_ready = False
|
||||
if self.rcp_corner_objects_180 is not None:
|
||||
vls_180 = self.rcp_corner_objects_180.update(can_strings)
|
||||
self.updated_corner_objects_180.update(vls_180)
|
||||
corner_180_ready = self.trigger_msg_corner_objects_180 in self.updated_corner_objects_180
|
||||
|
||||
corner_430_ready = False
|
||||
if self.rcp_corner_objects_430 is not None:
|
||||
vls_430 = self.rcp_corner_objects_430.update(can_strings)
|
||||
self.updated_corner_objects_430.update(vls_430)
|
||||
corner_430_ready = self.trigger_msg_corner_objects_430 in self.updated_corner_objects_430
|
||||
|
||||
scc_ready = not self.radar_tracks and self.frame % 5 == 0 and self.rcp_scc is not None
|
||||
|
||||
if track_ready:
|
||||
self._update(self.updated_tracks)
|
||||
self.updated_tracks.clear()
|
||||
|
||||
if corner_ready:
|
||||
self._update_corner_objects(self.updated_corner_objects)
|
||||
self.corner_object_missed_updates = 0
|
||||
self.updated_corner_objects.clear()
|
||||
|
||||
if corner_180_ready:
|
||||
self._update_corner_objects_180(self.updated_corner_objects_180)
|
||||
self.corner_object_180_missed_updates = 0
|
||||
self.updated_corner_objects_180.clear()
|
||||
|
||||
if corner_430_ready:
|
||||
self._update_corner_objects_430(self.updated_corner_objects_430)
|
||||
self.corner_object_430_missed_updates = 0
|
||||
self.updated_corner_objects_430.clear()
|
||||
|
||||
# Corner radar runs at its own cadence. Do not let corner-only frames publish
|
||||
# RadarData, since liveTracks uses a fixed radarTimeStep for aLead/jLead.
|
||||
publish_ready = track_ready or scc_ready
|
||||
if not publish_ready:
|
||||
return None
|
||||
|
||||
if self.rcp_scc is not None:
|
||||
self._update_scc(self.updated_scc)
|
||||
if self.rcp_corner_objects is not None:
|
||||
if self.updated_corner_objects:
|
||||
self._update_corner_objects(self.updated_corner_objects)
|
||||
self.corner_object_missed_updates = 0
|
||||
else:
|
||||
self.corner_object_missed_updates += 1
|
||||
if self.corner_object_missed_updates > 10:
|
||||
self._clear_corner_objects()
|
||||
if self.rcp_corner_objects_180 is not None:
|
||||
if self.updated_corner_objects_180:
|
||||
self._update_corner_objects_180(self.updated_corner_objects_180)
|
||||
self.corner_object_180_missed_updates = 0
|
||||
else:
|
||||
self.corner_object_180_missed_updates += 1
|
||||
if self.corner_object_180_missed_updates > 10:
|
||||
self._clear_corner_objects_180()
|
||||
if self.rcp_corner_objects_430 is not None:
|
||||
if self.updated_corner_objects_430:
|
||||
self._update_corner_objects_430(self.updated_corner_objects_430)
|
||||
self.corner_object_430_missed_updates = 0
|
||||
else:
|
||||
self.corner_object_430_missed_updates += 1
|
||||
if self.corner_object_430_missed_updates > 10:
|
||||
self._clear_corner_objects_430()
|
||||
self.updated_scc.clear()
|
||||
self.updated_corner_objects.clear()
|
||||
self.updated_corner_objects_180.clear()
|
||||
self.updated_corner_objects_430.clear()
|
||||
|
||||
ret = structs.RadarData()
|
||||
if ((self.rcp_tracks is not None and self.radar_tracks and not self.rcp_tracks.can_valid) or
|
||||
(self.rcp_scc is not None and not self.corner_objects_available and not self.rcp_scc.can_valid) or
|
||||
(self.rcp_corner_objects is not None and not self.rcp_corner_objects.can_valid) or
|
||||
(self.rcp_corner_objects_180 is not None and not self.rcp_corner_objects_180.can_valid) or
|
||||
(self.rcp_corner_objects_430 is not None and not self.rcp_corner_objects_430.can_valid)):
|
||||
ret.errors.canError = True
|
||||
ret.points = [point for point in self.pts.values() if point.measured]
|
||||
return ret
|
||||
|
||||
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.radar_group3:
|
||||
# Group 3 marks an empty object slot with LONG_DIST raw 0x7ff (204.7 m).
|
||||
valid = msg['LONG_DIST'] < 204.7
|
||||
elif self.canfd:
|
||||
valid = msg['VALID_CNT'] > 10
|
||||
elif self.radar_group4:
|
||||
# EN: DNMWR006 exposes eight stable tracked-object slots at 0x500-0x507.
|
||||
# Messages from 0x508 onward are distance-sorted raw detections without
|
||||
# stable IDs, so they are excluded. OBJECT_STATE 3 is a confirmed track;
|
||||
# empty slots use LONG_DIST raw 0xfff8 (409.55 m). Driving logs reached
|
||||
# 317.80 m, so 325 m preserves every observed confirmed track while
|
||||
# retaining margin from the empty-slot sentinel. Keep the +/-6 m
|
||||
# ego/adjacent-lane envelope to suppress farther roadside reflections.
|
||||
# KO: DNMWR006의 안정적인 추적 객체 슬롯은 0x500~0x507의 8개임.
|
||||
# 0x508 이후 메시지는 고정 ID가 없는 거리순 raw detection이므로 제외함.
|
||||
# OBJECT_STATE 3은 확정 추적 객체이며, 빈 슬롯은 LONG_DIST raw
|
||||
# 0xfff8(409.55m)을 사용함. 주행 로그의 최대값은 317.80m였으므로
|
||||
# 325m 상한으로 관측된 확정 트랙을 모두 보존하면서 빈 슬롯 값과 충분한
|
||||
# 여유를 확보함. 원거리 도로변 반사를 줄이기 위해 좌우 6m 범위를 유지함.
|
||||
valid = (msg['OBJECT_STATE'] == 3 and 0.2 < msg['LONG_DIST'] < RADAR_GROUP4_MAX_LONG_DIST and
|
||||
abs(msg['LAT_DIST']) <= RADAR_GROUP4_MAX_YREL)
|
||||
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:
|
||||
if self.radar_group3:
|
||||
# Group 3 reports the object's center. Convert it to the rear surface to match SCC/vision dRel.
|
||||
self.pts[t_id].dRel = max(0.0, msg['LONG_DIST'] - msg['OBJECT_LENGTH'] * 0.5 - 0.1)
|
||||
else:
|
||||
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 = float('nan') if self.radar_group3 else msg['REL_ACCEL']
|
||||
self.pts[t_id].yvRel = 0.0 if self.radar_group3 else msg['LAT_SPEED']
|
||||
elif self.radar_group4:
|
||||
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 = float('nan')
|
||||
self.pts[t_id].yvRel = 0.0
|
||||
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_corner_objects(self, updated_messages):
|
||||
if self.rcp_corner_objects is None:
|
||||
return
|
||||
|
||||
if not updated_messages:
|
||||
self._clear_corner_objects()
|
||||
return
|
||||
|
||||
candidates = []
|
||||
for slot, addr in enumerate(range(CORNER_OBJECT_235_START_ADDR, CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT)):
|
||||
t_id = CORNER_OBJECT_235_TRACK_ID_OFFSET + slot
|
||||
msg = self.rcp_corner_objects.vl[f"CORNER_RADAR_235_OBJECTS_{addr:x}"]
|
||||
|
||||
d_rel = msg["OBJ_REL_POS_X"]
|
||||
y_rel = msg["OBJ_REL_POS_Y"]
|
||||
v_rel = msg["OBJ_REL_VEL_X"]
|
||||
yv_rel = msg["OBJ_REL_VEL_Y"]
|
||||
a_rel = msg["OBJ_REL_ACCEL_X"]
|
||||
# Side objects are clipped to x=0 by the corner radar. Quality, identity,
|
||||
# and lateral motion still describe a real object, so keep them for
|
||||
# corner-confirmed front-radar association in radard.
|
||||
valid = msg["OBJ_QUAL_LEVEL"] > 0 and corner_object_position_valid(d_rel, y_rel) and v_rel > -99.0
|
||||
|
||||
if not valid:
|
||||
continue
|
||||
candidates.append((t_id, int(msg["OBJ_OBJECT_ID"]), int(msg["OBJ_AGE"]), int(msg["OBJ_QUAL_LEVEL"]),
|
||||
d_rel, y_rel, v_rel, yv_rel, a_rel))
|
||||
|
||||
self._apply_corner_objects("corner235", candidates,
|
||||
range(CORNER_OBJECT_235_TRACK_ID_OFFSET,
|
||||
CORNER_OBJECT_235_TRACK_ID_OFFSET + CORNER_OBJECT_235_MSG_COUNT))
|
||||
|
||||
def _update_corner_objects_180(self, updated_messages):
|
||||
if self.rcp_corner_objects_180 is None:
|
||||
return
|
||||
|
||||
if not updated_messages:
|
||||
self._clear_corner_objects_180()
|
||||
return
|
||||
|
||||
candidates = []
|
||||
for msg_index, addr in enumerate(range(CORNER_OBJECT_180_START_ADDR, CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT)):
|
||||
msg = self.rcp_corner_objects_180.vl[f"CORNER_RADAR_180_OBJECTS_{addr:x}"]
|
||||
for slot_index in range(CORNER_OBJECT_180_SLOTS_PER_MSG):
|
||||
t_id = CORNER_OBJECT_180_TRACK_ID_OFFSET + msg_index * CORNER_OBJECT_180_SLOTS_PER_MSG + slot_index
|
||||
prefix = f"SLOT{slot_index + 1}_"
|
||||
d_rel = msg[f"{prefix}REL_POS_X"]
|
||||
y_rel = msg[f"{prefix}REL_POS_Y"]
|
||||
v_rel = msg[f"{prefix}REL_VEL_X"]
|
||||
yv_rel = msg[f"{prefix}REL_VEL_Y"]
|
||||
a_rel = msg[f"{prefix}REL_ACCEL_X"]
|
||||
valid = msg[f"{prefix}QUAL_LEVEL"] > 0 and corner_object_position_valid(d_rel, y_rel) and v_rel > -99.0
|
||||
|
||||
if not valid:
|
||||
continue
|
||||
candidates.append((t_id, int(msg[f"{prefix}OBJECT_ID"]), int(msg[f"{prefix}AGE"]), int(msg[f"{prefix}QUAL_LEVEL"]),
|
||||
d_rel, y_rel, v_rel, yv_rel, a_rel))
|
||||
|
||||
self._apply_corner_objects("corner180", candidates,
|
||||
range(CORNER_OBJECT_180_TRACK_ID_OFFSET,
|
||||
CORNER_OBJECT_180_TRACK_ID_OFFSET + CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG))
|
||||
|
||||
def _apply_corner_objects(self, source, candidates, slot_ids):
|
||||
for t_id in slot_ids:
|
||||
self._clear_point(t_id)
|
||||
|
||||
# The same object can occupy two CAN slots for one cycle during a slot handoff.
|
||||
# Publish only the newest/highest-quality copy so trackId stays unique.
|
||||
objects = {}
|
||||
for candidate in candidates:
|
||||
object_id = candidate[1]
|
||||
previous = objects.get(object_id)
|
||||
if previous is None or (candidate[2], candidate[3]) > (previous[2], previous[3]):
|
||||
objects[object_id] = candidate
|
||||
|
||||
for t_id, object_id, age, _, d_rel, y_rel, v_rel, yv_rel, a_rel in objects.values():
|
||||
point = self.pts[t_id]
|
||||
point.measured = True
|
||||
point.trackId = self.corner_object_track_ids.get_track_id(source, object_id, age)
|
||||
point.radarSource = source
|
||||
point.dRel = d_rel
|
||||
point.yRel = y_rel
|
||||
point.vRel = v_rel
|
||||
point.vLead = v_rel + self.v_ego
|
||||
point.aRel = a_rel
|
||||
point.yvRel = yv_rel
|
||||
|
||||
|
||||
def _update_corner_objects_430(self, updated_messages):
|
||||
if self.rcp_corner_objects_430 is None:
|
||||
return
|
||||
|
||||
if not updated_messages:
|
||||
self._clear_corner_objects_430()
|
||||
return
|
||||
|
||||
bank_defs = (
|
||||
(CORNER_OBJECT_430_LEFT_START_ADDR, 1.0, 0),
|
||||
(CORNER_OBJECT_430_RIGHT_START_ADDR, -1.0, CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * CORNER_OBJECT_430_SLOTS_PER_MSG),
|
||||
)
|
||||
for start_addr, side_sign, track_base in bank_defs:
|
||||
bins = []
|
||||
for msg_index, addr in enumerate(range(start_addr, start_addr + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)):
|
||||
msg = self.rcp_corner_objects_430.vl[f"CORNER_RADAR_430_OBJECTS_{addr:x}"]
|
||||
for slot_index in range(CORNER_OBJECT_430_SLOTS_PER_MSG):
|
||||
prefix = f"SLOT{slot_index + 1}_"
|
||||
distance_raw = int(msg[f"{prefix}DISTANCE_RAW"])
|
||||
raw = (
|
||||
distance_raw |
|
||||
(int(msg[f"{prefix}META_13_15"]) << 13) |
|
||||
(int(msg[f"{prefix}META_BYTE_2"]) << 16) |
|
||||
(int(msg[f"{prefix}META_BYTE_3"]) << 24)
|
||||
)
|
||||
code = (
|
||||
int(msg[f"{prefix}META_13_15"]),
|
||||
int(msg[f"{prefix}META_BYTE_2"]),
|
||||
int(msg[f"{prefix}META_BYTE_3"]),
|
||||
)
|
||||
d_rel = distance_raw * 0.05
|
||||
default_distance = CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MIN <= distance_raw <= CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MAX
|
||||
base_valid = (
|
||||
raw not in CORNER_OBJECT_430_EMPTY_RAW_VALUES and
|
||||
distance_raw not in (0, 8000, 8191) and
|
||||
not default_distance and
|
||||
0.2 < d_rel < CORNER_OBJECT_430_MAX_DREL
|
||||
)
|
||||
candidate_valid = (
|
||||
base_valid and
|
||||
slot_index + 1 not in CORNER_OBJECT_430_CANDIDATE_EXCLUDED_SLOTS and
|
||||
code[2] in CORNER_OBJECT_430_CANDIDATE_META_BYTE_3 and
|
||||
code[1] in CORNER_OBJECT_430_STRONG_META_BYTE_2 + CORNER_OBJECT_430_WEAK_META_BYTE_2
|
||||
)
|
||||
bins.append({
|
||||
"msg_index": msg_index,
|
||||
"slot_index": slot_index,
|
||||
"distance_raw": distance_raw,
|
||||
"d_rel": d_rel,
|
||||
"code": code,
|
||||
"candidate_valid": candidate_valid,
|
||||
})
|
||||
|
||||
supported_bins = []
|
||||
candidates = [b for b in bins if b["candidate_valid"]]
|
||||
for b in candidates:
|
||||
support = 1
|
||||
for other in candidates:
|
||||
if other is b:
|
||||
continue
|
||||
if abs(other["msg_index"] - b["msg_index"]) > 1:
|
||||
continue
|
||||
if abs(other["slot_index"] - b["slot_index"]) > 2:
|
||||
continue
|
||||
if abs(other["distance_raw"] - b["distance_raw"]) > CORNER_OBJECT_430_CANDIDATE_RAW_DELTA:
|
||||
continue
|
||||
support += 1
|
||||
min_support = (CORNER_OBJECT_430_STRONG_MIN_SUPPORT if b["code"][1] in CORNER_OBJECT_430_STRONG_META_BYTE_2
|
||||
else CORNER_OBJECT_430_WEAK_MIN_SUPPORT)
|
||||
if support >= min_support:
|
||||
supported_bins.append({**b, "support": support})
|
||||
|
||||
clusters = []
|
||||
for b in sorted(supported_bins, key=lambda item: item["distance_raw"]):
|
||||
if not clusters or b["distance_raw"] - clusters[-1][-1]["distance_raw"] > CORNER_OBJECT_430_CLUSTER_RAW_GAP:
|
||||
clusters.append([b])
|
||||
else:
|
||||
clusters[-1].append(b)
|
||||
clusters = sorted(clusters, key=lambda cluster: sum(b["distance_raw"] for b in cluster) / len(cluster))[:CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE]
|
||||
|
||||
cluster_objects = []
|
||||
for cluster in clusters:
|
||||
msg_index = sum(b["msg_index"] for b in cluster) / len(cluster)
|
||||
slot = sum(b["slot_index"] + 1 for b in cluster) / len(cluster)
|
||||
lateral_cell = (CORNER_OBJECT_430_LATERAL_CELL_MSG_WEIGHT * msg_index +
|
||||
CORNER_OBJECT_430_LATERAL_CELL_SLOT_WEIGHT * slot)
|
||||
mapped_cell = lateral_cell if side_sign > 0.0 else CORNER_OBJECT_430_RIGHT_CELL_MIRROR - lateral_cell
|
||||
y_abs = max(CORNER_OBJECT_430_MIN_ABS_YREL,
|
||||
min(CORNER_OBJECT_430_MAX_ABS_YREL,
|
||||
CORNER_OBJECT_430_YREL_OFFSET - CORNER_OBJECT_430_YREL_SCALE * mapped_cell))
|
||||
cluster_objects.append({
|
||||
"d_rel": sum(b["d_rel"] for b in cluster) / len(cluster),
|
||||
"y_rel": side_sign * y_abs,
|
||||
"code": max((b["code"] for b in cluster), key=lambda code: sum(1 for item in cluster if item["code"] == code)),
|
||||
})
|
||||
|
||||
active_t_ids = set()
|
||||
side_track_ids = [
|
||||
CORNER_OBJECT_430_TRACK_ID_OFFSET + track_base + slot
|
||||
for slot in range(CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE)
|
||||
]
|
||||
unmatched_track_ids = {t_id for t_id in side_track_ids if t_id in self.corner_object_430_prev_d_rel}
|
||||
unused_track_ids = [t_id for t_id in side_track_ids if t_id not in unmatched_track_ids]
|
||||
|
||||
for cluster in cluster_objects:
|
||||
d_rel = cluster["d_rel"]
|
||||
code = cluster["code"]
|
||||
matched_t_id = None
|
||||
if unmatched_track_ids:
|
||||
nearest_t_id = min(unmatched_track_ids, key=lambda t_id: abs(d_rel - self.corner_object_430_prev_d_rel[t_id]))
|
||||
if abs(d_rel - self.corner_object_430_prev_d_rel[nearest_t_id]) <= CORNER_OBJECT_430_TRACK_MATCH_MAX_DREL_DELTA:
|
||||
matched_t_id = nearest_t_id
|
||||
unmatched_track_ids.remove(matched_t_id)
|
||||
if matched_t_id is None and unused_track_ids:
|
||||
matched_t_id = unused_track_ids.pop(0)
|
||||
if matched_t_id is None:
|
||||
continue
|
||||
|
||||
t_id = matched_t_id
|
||||
active_t_ids.add(t_id)
|
||||
prev_d_rel = self.corner_object_430_prev_d_rel.get(t_id)
|
||||
prev_code = self.corner_object_430_prev_code.get(t_id)
|
||||
self.corner_object_430_prev_d_rel[t_id] = d_rel
|
||||
self.corner_object_430_prev_y_rel[t_id] = cluster["y_rel"]
|
||||
self.corner_object_430_prev_code[t_id] = code
|
||||
reset_track = prev_d_rel is None or code != prev_code or abs(d_rel - prev_d_rel) > CORNER_OBJECT_430_MAX_DREL_DELTA
|
||||
if reset_track:
|
||||
self.corner_object_430_prev_v_rel.pop(t_id, None)
|
||||
self.corner_object_430_prev_yv_rel.pop(t_id, None)
|
||||
self.corner_object_430_history.pop(t_id, None)
|
||||
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
|
||||
|
||||
history = self.corner_object_430_history.setdefault(t_id, deque(maxlen=CORNER_OBJECT_430_HISTORY_SIZE))
|
||||
history.append((d_rel, cluster["y_rel"]))
|
||||
if len(history) < CORNER_OBJECT_430_MIN_HISTORY:
|
||||
self._clear_point(t_id)
|
||||
continue
|
||||
|
||||
window_dt = CORNER_OBJECT_430_DT * (len(history) - 1)
|
||||
first_d_rel, first_y_rel = history[0]
|
||||
hist_v_rel = (d_rel - first_d_rel) / window_dt
|
||||
if abs(hist_v_rel) > CORNER_OBJECT_430_MAX_ABS_VREL:
|
||||
self.corner_object_430_prev_v_rel.pop(t_id, None)
|
||||
self.corner_object_430_prev_yv_rel.pop(t_id, None)
|
||||
self.corner_object_430_history.pop(t_id, None)
|
||||
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
|
||||
self._clear_point(t_id)
|
||||
continue
|
||||
prev_v_rel = self.corner_object_430_prev_v_rel.get(t_id, hist_v_rel)
|
||||
v_rel = (1.0 - CORNER_OBJECT_430_VREL_ALPHA) * prev_v_rel + CORNER_OBJECT_430_VREL_ALPHA * hist_v_rel
|
||||
self.corner_object_430_prev_v_rel[t_id] = v_rel
|
||||
|
||||
inward_steps = 0
|
||||
usable_steps = 0
|
||||
prev_abs_y = abs(history[0][1])
|
||||
for _, y_rel in list(history)[1:]:
|
||||
abs_y = abs(y_rel)
|
||||
delta = prev_abs_y - abs_y
|
||||
if abs(delta) > 1e-3:
|
||||
usable_steps += 1
|
||||
if delta > 0.0:
|
||||
inward_steps += 1
|
||||
prev_abs_y = abs_y
|
||||
net_inward_y = abs(first_y_rel) - abs(cluster["y_rel"])
|
||||
inward_ratio = inward_steps / usable_steps if usable_steps > 0 else 0.0
|
||||
hist_yv_rel = (cluster["y_rel"] - first_y_rel) / window_dt
|
||||
recent_inward_y = abs(history[-3][1]) - abs(cluster["y_rel"]) if len(history) >= 3 else net_inward_y
|
||||
if (net_inward_y < CORNER_OBJECT_430_MIN_INWARD_YREL_DELTA or
|
||||
recent_inward_y < CORNER_OBJECT_430_MIN_RECENT_INWARD_YREL_DELTA or
|
||||
inward_ratio < CORNER_OBJECT_430_MIN_INWARD_RATIO or
|
||||
abs(hist_yv_rel) > CORNER_OBJECT_430_MAX_ABS_YVREL):
|
||||
hist_yv_rel = 0.0
|
||||
inward_motion_candidate = hist_yv_rel != 0.0 and abs(cluster["y_rel"]) <= CORNER_OBJECT_430_INWARD_KEEP_YVREL_ABS_YREL
|
||||
inward_center_candidate = inward_motion_candidate and abs(cluster["y_rel"]) <= CORNER_OBJECT_430_INWARD_CENTER_ABS_YREL
|
||||
y_rel = cluster["y_rel"]
|
||||
if inward_motion_candidate:
|
||||
if inward_center_candidate:
|
||||
self.corner_object_430_noncenter_inward_frames[t_id] = 0
|
||||
prev_yv_rel = self.corner_object_430_prev_yv_rel.get(t_id, hist_yv_rel)
|
||||
yv_rel = (1.0 - CORNER_OBJECT_430_YVREL_ALPHA) * prev_yv_rel + CORNER_OBJECT_430_YVREL_ALPHA * hist_yv_rel
|
||||
else:
|
||||
noncenter_frames = self.corner_object_430_noncenter_inward_frames.get(t_id, 0) + 1
|
||||
self.corner_object_430_noncenter_inward_frames[t_id] = noncenter_frames
|
||||
if noncenter_frames <= CORNER_OBJECT_430_EARLY_INWARD_NONCENTER_FRAMES:
|
||||
prev_yv_rel = self.corner_object_430_prev_yv_rel.get(t_id, hist_yv_rel)
|
||||
yv_rel = (1.0 - CORNER_OBJECT_430_YVREL_ALPHA) * prev_yv_rel + CORNER_OBJECT_430_YVREL_ALPHA * hist_yv_rel
|
||||
else:
|
||||
yv_rel = 0.0
|
||||
if not inward_center_candidate and abs(y_rel) < CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL:
|
||||
y_rel = math.copysign(CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL, y_rel)
|
||||
else:
|
||||
hist_yv_rel = 0.0
|
||||
yv_rel = 0.0
|
||||
self.corner_object_430_noncenter_inward_frames[t_id] = 0
|
||||
if abs(y_rel) < CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL:
|
||||
y_rel = math.copysign(CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL, y_rel)
|
||||
self.corner_object_430_prev_yv_rel[t_id] = yv_rel
|
||||
|
||||
self.pts[t_id].measured = True
|
||||
self.pts[t_id].trackId = t_id
|
||||
self.pts[t_id].dRel = d_rel
|
||||
self.pts[t_id].yRel = y_rel
|
||||
self.pts[t_id].vRel = v_rel
|
||||
self.pts[t_id].vLead = v_rel + self.v_ego
|
||||
self.pts[t_id].aRel = float('nan')
|
||||
self.pts[t_id].yvRel = yv_rel
|
||||
|
||||
side_track_count = CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * CORNER_OBJECT_430_SLOTS_PER_MSG
|
||||
for slot in range(side_track_count):
|
||||
t_id = CORNER_OBJECT_430_TRACK_ID_OFFSET + track_base + slot
|
||||
if t_id in active_t_ids:
|
||||
continue
|
||||
self.corner_object_430_prev_d_rel.pop(t_id, None)
|
||||
self.corner_object_430_prev_v_rel.pop(t_id, None)
|
||||
self.corner_object_430_prev_y_rel.pop(t_id, None)
|
||||
self.corner_object_430_prev_yv_rel.pop(t_id, None)
|
||||
self.corner_object_430_prev_code.pop(t_id, None)
|
||||
self.corner_object_430_history.pop(t_id, None)
|
||||
self.corner_object_430_noncenter_inward_frames.pop(t_id, None)
|
||||
self._clear_point(t_id)
|
||||
|
||||
|
||||
def _clear_point(self, t_id):
|
||||
self.pts[t_id].measured = False
|
||||
self.pts[t_id].dRel = 0
|
||||
self.pts[t_id].yRel = 0
|
||||
self.pts[t_id].vRel = 0
|
||||
self.pts[t_id].vLead = self.v_ego
|
||||
self.pts[t_id].aRel = float('nan')
|
||||
self.pts[t_id].yvRel = 0
|
||||
|
||||
def _clear_corner_objects(self):
|
||||
for slot in range(CORNER_OBJECT_235_MSG_COUNT):
|
||||
self._clear_point(CORNER_OBJECT_235_TRACK_ID_OFFSET + slot)
|
||||
self.corner_object_track_ids.clear_source("corner235")
|
||||
|
||||
def _clear_corner_objects_180(self):
|
||||
for slot in range(CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG):
|
||||
self._clear_point(CORNER_OBJECT_180_TRACK_ID_OFFSET + slot)
|
||||
self.corner_object_track_ids.clear_source("corner180")
|
||||
|
||||
def _clear_corner_objects_430(self):
|
||||
self.corner_object_430_prev_d_rel.clear()
|
||||
self.corner_object_430_prev_v_rel.clear()
|
||||
self.corner_object_430_prev_y_rel.clear()
|
||||
self.corner_object_430_prev_yv_rel.clear()
|
||||
self.corner_object_430_prev_code.clear()
|
||||
self.corner_object_430_history.clear()
|
||||
self.corner_object_430_noncenter_inward_frames.clear()
|
||||
for slot in range(CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * 2 * CORNER_OBJECT_430_SLOTS_PER_MSG):
|
||||
self._clear_point(CORNER_OBJECT_430_TRACK_ID_OFFSET + slot)
|
||||
|
||||
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,0 +1,21 @@
|
||||
#!/usr/bin/env python3
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.hyundai.values import PLATFORM_CODE_ECUS, get_platform_codes
|
||||
from iqdbc.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}')
|
||||
@@ -0,0 +1,194 @@
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from iqdbc.can import CANPacker, CANParser
|
||||
from iqdbc.car import Bus, gen_empty_fingerprint, structs
|
||||
from iqdbc.car.hyundai.carstate import CarState, EV_MODE_STATUS_TIMEOUT_NS, _get_ev_mode_state
|
||||
from iqdbc.car.hyundai.interface import CarInterface
|
||||
from iqdbc.car.hyundai.values import CANFD_HYBRID_STATUS_ADDR, CANFD_HYBRID_STATUS_DLC, CAR, DBC, EV_MODE_ACTIVE_VALUES, \
|
||||
EV_MODE_STATUS_ADDR, EV_MODE_STATUS_DLC, EV_MODE_STATUS_MSG, EV_MODE_STATUS_SIGNAL, \
|
||||
HyundaiExtFlags, HyundaiFlags
|
||||
from iqpilot.common.params import Params
|
||||
|
||||
|
||||
def get_params(candidate, *, hybrid=True, hybrid_bus=0, hybrid_status=None, hybrid_status_bus=0,
|
||||
hybrid_status_dlc=CANFD_HYBRID_STATUS_DLC, status_bus=0, status_dlc=EV_MODE_STATUS_DLC):
|
||||
Params().put_int("HyundaiCameraSCC", 1)
|
||||
Params().put_int("CanfdHDA2", 1)
|
||||
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
if hybrid:
|
||||
fingerprint[hybrid_bus][0x105] = 32
|
||||
if hybrid_status is None:
|
||||
hybrid_status = hybrid
|
||||
if hybrid_status:
|
||||
fingerprint[hybrid_status_bus][CANFD_HYBRID_STATUS_ADDR] = hybrid_status_dlc
|
||||
if status_bus is not None:
|
||||
fingerprint[status_bus][EV_MODE_STATUS_ADDR] = status_dlc
|
||||
return CarInterface.get_params(candidate, fingerprint, [], False, False, False)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("candidate", "hybrid", "status_bus", "status_dlc", "expected"), (
|
||||
(CAR.HYUNDAI_SANTAFE_MX5_HEV, True, 0, 32, True),
|
||||
(CAR.HYUNDAI_SANTAFE_MX5_HEV, False, 0, 32, False),
|
||||
(CAR.HYUNDAI_SANTAFE_MX5_HEV, True, None, 32, False),
|
||||
(CAR.HYUNDAI_SANTAFE_MX5_HEV, True, 1, 32, False),
|
||||
(CAR.HYUNDAI_SANTAFE_MX5_HEV, True, 0, 16, False),
|
||||
(CAR.KIA_SORENTO_4TH_GEN, False, None, 32, False),
|
||||
# Shared ICE/HEV/PHEV candidates rely on the observed ECAN capability frames, not their model name.
|
||||
(CAR.HYUNDAI_TUCSON_4TH_GEN, True, 0, 32, True),
|
||||
(CAR.HYUNDAI_TUCSON_4TH_GEN, False, 0, 32, False),
|
||||
(CAR.KIA_SORENTO_HEV_4TH_GEN, True, 0, 32, True),
|
||||
(CAR.HYUNDAI_KONA_HEV_2ND_GEN, True, 0, 32, True),
|
||||
(CAR.HYUNDAI_KONA_HEV_2ND_GEN, False, 0, 32, False),
|
||||
(CAR.HYUNDAI_ELANTRA_HEV_2021, True, 0, 32, False),
|
||||
))
|
||||
def test_ev_mode_capability(candidate, hybrid, status_bus, status_dlc, expected):
|
||||
CP = get_params(candidate, hybrid=hybrid, status_bus=status_bus, status_dlc=status_dlc)
|
||||
assert bool(CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230) == expected
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("hybrid_status_bus", "hybrid_status_dlc"), (
|
||||
(1, CANFD_HYBRID_STATUS_DLC),
|
||||
(0, 16),
|
||||
))
|
||||
def test_ev_mode_capability_requires_ecan_hybrid_status_dlc32(hybrid_status_bus, hybrid_status_dlc):
|
||||
CP = get_params(CAR.HYUNDAI_SANTAFE_MX5_HEV, hybrid_status_bus=hybrid_status_bus,
|
||||
hybrid_status_dlc=hybrid_status_dlc)
|
||||
|
||||
assert not bool(CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230)
|
||||
|
||||
|
||||
def test_0x105_and_ev_status_without_0xfa_do_not_enable_ev_mode():
|
||||
CP = get_params(CAR.HYUNDAI_TUCSON_4TH_GEN, hybrid=True, hybrid_status=False)
|
||||
|
||||
assert bool(CP.flags & HyundaiFlags.HYBRID)
|
||||
assert not bool(CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230)
|
||||
|
||||
|
||||
def test_ev_mode_display_capability_does_not_change_hybrid_safety_classification():
|
||||
CP = get_params(CAR.HYUNDAI_TUCSON_4TH_GEN, hybrid=False, hybrid_status=True)
|
||||
|
||||
assert not bool(CP.flags & HyundaiFlags.HYBRID)
|
||||
assert bool(CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230)
|
||||
|
||||
|
||||
def test_ev_mode_capability_uses_the_detected_ecan_offset():
|
||||
CP = get_params(CAR.KIA_SORENTO_HEV_4TH_GEN, hybrid_bus=4, hybrid_status_bus=4, status_bus=4)
|
||||
|
||||
assert bool(CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230)
|
||||
|
||||
|
||||
def test_ev_mode_parser_registration_is_capability_gated():
|
||||
supported = get_params(CAR.HYUNDAI_SANTAFE_MX5_HEV)
|
||||
unsupported = get_params(CAR.KIA_SORENTO_4TH_GEN, hybrid=False, status_bus=1, status_dlc=16)
|
||||
|
||||
supported_parser = CarState.get_can_parsers_canfd(None, supported)[Bus.pt]
|
||||
unsupported_parser = CarState.get_can_parsers_canfd(None, unsupported)[Bus.pt]
|
||||
|
||||
assert EV_MODE_STATUS_ADDR in supported_parser.addresses
|
||||
assert supported_parser.message_states[EV_MODE_STATUS_ADDR].ignore_alive
|
||||
assert supported_parser.message_states[EV_MODE_STATUS_ADDR].ignore_counter
|
||||
assert EV_MODE_STATUS_ADDR not in unsupported_parser.addresses
|
||||
|
||||
|
||||
def test_sorento_ice_corner_radar_status_does_not_enable_ev_mode():
|
||||
Params().put_int("HyundaiCameraSCC", 1)
|
||||
Params().put_int("CanfdHDA2", 1)
|
||||
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[1][0x230] = 16 # Actual Sorento ICE ACAN corner-radar status frame.
|
||||
CP = CarInterface.get_params(CAR.KIA_SORENTO_4TH_GEN, fingerprint, [], False, False, False)
|
||||
parser = CarState.get_can_parsers_canfd(None, CP)[Bus.pt]
|
||||
|
||||
assert not bool(CP.extFlags & HyundaiExtFlags.EV_MODE_STATUS_230)
|
||||
assert EV_MODE_STATUS_ADDR not in parser.addresses
|
||||
|
||||
|
||||
def test_ev_mode_state_requires_a_fresh_dlc32_frame():
|
||||
CP = get_params(CAR.HYUNDAI_SANTAFE_MX5_HEV)
|
||||
parser = CarState.get_can_parsers_canfd(None, CP)[Bus.pt]
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
assert _get_ev_mode_state(parser) == (False, False)
|
||||
|
||||
timestamp = 1_000_000_000
|
||||
wrong_bus_short_status = (EV_MODE_STATUS_ADDR, b"\x00" * 16, 1)
|
||||
parser.update([timestamp, [wrong_bus_short_status]])
|
||||
assert _get_ev_mode_state(parser) == (False, False)
|
||||
|
||||
ev_active = packer.make_can_msg(EV_MODE_STATUS_MSG, parser.bus, {"COUNTER": 1, EV_MODE_STATUS_SIGNAL: 6})
|
||||
parser.update([timestamp + 100_000_000, [ev_active]])
|
||||
assert _get_ev_mode_state(parser) == (True, True)
|
||||
|
||||
ev_inactive = packer.make_can_msg(EV_MODE_STATUS_MSG, parser.bus, {"COUNTER": 2, EV_MODE_STATUS_SIGNAL: 3})
|
||||
parser.update([timestamp + 200_000_000, [ev_inactive]])
|
||||
assert _get_ev_mode_state(parser) == (False, True)
|
||||
|
||||
parser.dat[EV_MODE_STATUS_ADDR] = b"\x00" * 16
|
||||
assert _get_ev_mode_state(parser) == (False, False)
|
||||
|
||||
parser.dat[EV_MODE_STATUS_ADDR] = ev_inactive[1]
|
||||
parser.update([timestamp + 200_000_000 + EV_MODE_STATUS_TIMEOUT_NS + 1, []])
|
||||
assert not parser.bus_timeout
|
||||
assert _get_ev_mode_state(parser) == (False, False)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("mode", range(16))
|
||||
def test_ev_mode_enum_mapping(mode):
|
||||
CP = get_params(CAR.HYUNDAI_SANTAFE_MX5_HEV)
|
||||
parser = CarState.get_can_parsers_canfd(None, CP)[Bus.pt]
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
msg = packer.make_can_msg(EV_MODE_STATUS_MSG, parser.bus, {"COUNTER": mode, EV_MODE_STATUS_SIGNAL: mode})
|
||||
|
||||
parser.update([1_000_000_000, [msg]])
|
||||
|
||||
assert int(parser.vl[EV_MODE_STATUS_MSG][EV_MODE_STATUS_SIGNAL]) == mode
|
||||
assert _get_ev_mode_state(parser) == (mode in EV_MODE_ACTIVE_VALUES, True)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("payload", "expected_mode", "expected_active"), (
|
||||
("758139048402000000000000000000000000d3009805001c24000000c05dc80f", 1, True),
|
||||
("4d466f048402000000000000000000000000bf007408001c24000000c05dc80f", 2, True),
|
||||
("32206e048402000000000000000000000000c9007418001c24000000c05dc80f", 6, True),
|
||||
# Route 455 mode 3 was a false positive with the old single-bit 0x230 interpretation.
|
||||
("4f935e0484020000000000000000000000004400640d001c10000000c05dc40f", 3, False),
|
||||
("066558048402000000000000000000000000c4003020001c24000000c05dc80f", 8, False),
|
||||
("059683048402000000000000000000000000a1008425001c24000000c05dc80f", 9, False),
|
||||
("011305048402000000000000000000000000d600a829001c24000000c05dc80f", 10, False),
|
||||
))
|
||||
def test_ev_mode_dbc_decodes_real_mx5_frames(payload, expected_mode, expected_active):
|
||||
parser = CANParser("hyundai_canfd_generated", [(EV_MODE_STATUS_MSG, math.nan)], 0)
|
||||
updated = parser.update([1_000_000_000, [(EV_MODE_STATUS_ADDR, bytes.fromhex(payload), 0)]])
|
||||
|
||||
assert updated == {EV_MODE_STATUS_ADDR}
|
||||
assert int(parser.vl[EV_MODE_STATUS_MSG][EV_MODE_STATUS_SIGNAL]) == expected_mode
|
||||
assert _get_ev_mode_state(parser) == (expected_active, True)
|
||||
|
||||
|
||||
def test_ev_mode_rejects_checksum_corruption():
|
||||
CP = get_params(CAR.HYUNDAI_SANTAFE_MX5_HEV)
|
||||
parser = CarState.get_can_parsers_canfd(None, CP)[Bus.pt]
|
||||
payload = bytearray.fromhex("32206e048402000000000000000000000000c9007418001c24000000c05dc80f")
|
||||
payload[-1] ^= 1
|
||||
|
||||
parser.update([1_000_000_000, [(EV_MODE_STATUS_ADDR, bytes(payload), parser.bus)]])
|
||||
|
||||
assert EV_MODE_STATUS_ADDR not in parser.dat
|
||||
assert _get_ev_mode_state(parser) == (False, False)
|
||||
|
||||
|
||||
def test_ev_mode_fields_default_invalid():
|
||||
state = structs.CarState()
|
||||
assert not state.evModeActive
|
||||
assert not state.evModeValid
|
||||
|
||||
|
||||
def test_ev_mode_parser_is_optional_for_can_validity():
|
||||
CP = get_params(CAR.HYUNDAI_SANTAFE_MX5_HEV)
|
||||
parser = CarState.get_can_parsers_canfd(None, CP)[Bus.pt]
|
||||
state = parser.message_states[EV_MODE_STATUS_ADDR]
|
||||
|
||||
assert state.ignore_alive
|
||||
assert state.ignore_counter
|
||||
@@ -0,0 +1,225 @@
|
||||
from hypothesis import settings, given, strategies as st
|
||||
|
||||
from iqdbc.car import gen_empty_fingerprint
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.fw_versions import build_fw_dict
|
||||
from iqdbc.car.hyundai.interface import CarInterface
|
||||
from iqdbc.car.hyundai.radar_interface import RADAR_START_ADDR
|
||||
from iqdbc.car.hyundai.values import CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \
|
||||
HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, \
|
||||
PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \
|
||||
HyundaiFlags, get_platform_codes, HyundaiSafetyFlags
|
||||
from iqdbc.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, monkeypatch, tmp_path):
|
||||
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
|
||||
# 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, 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, 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 cars are not classified with classic-CAN-only parsing or safety modes.
|
||||
can_specific_feature_list = set.union(*CAN_GEARS.values(), *CHECKSUM.values(), LEGACY_SAFETY_MODE_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 HYBRID_CAR <= set(CAR)
|
||||
assert EV_CAR <= set(CAR)
|
||||
|
||||
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.get(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():
|
||||
if car_model == CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN:
|
||||
continue
|
||||
with subtests.test(car_model=car_model.value):
|
||||
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:
|
||||
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):
|
||||
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)
|
||||
|
||||
# A unique fuzzy match must always resolve back to the platform that supplied the firmware.
|
||||
# Ambiguity is expected for shared platform codes and for platforms without parseable dates.
|
||||
assert {CAR.GENESIS_G70, CAR.GENESIS_G70_2020} <= platforms_with_shared_codes
|
||||
@@ -0,0 +1,53 @@
|
||||
import pytest
|
||||
|
||||
from iqdbc.car import gen_empty_fingerprint, structs
|
||||
from iqdbc.car.hyundai.interface import CarInterface
|
||||
from iqdbc.car.hyundai.values import CAR, HyundaiFlagsIQ, HyundaiSafetyFlagsIQ
|
||||
|
||||
|
||||
@pytest.mark.parametrize("candidate", list(CAR), ids=lambda candidate: candidate.value)
|
||||
@pytest.mark.parametrize("alpha_long", (False, True), ids=("stock_long", "openpilot_long"))
|
||||
def test_all_platform_state_controller_and_radar(candidate, alpha_long, monkeypatch, tmp_path):
|
||||
"""Every declared HKG platform must initialize and execute one complete interface cycle."""
|
||||
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path / candidate.value))
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
|
||||
cp = CarInterface.get_params(candidate, fingerprint, [], alpha_long, False, False)
|
||||
cp_iq = CarInterface.get_params_iq(cp, candidate, fingerprint, [], alpha_long, False, False)
|
||||
interface = CarInterface(cp, cp_iq)
|
||||
|
||||
state, state_iq = interface.update([])
|
||||
actuators, can_sends = interface.apply(structs.CarControl().as_reader(), structs.IQCarControl())
|
||||
radar = interface.RadarInterface(cp, cp_iq)
|
||||
radar_result = radar.update([])
|
||||
|
||||
assert state.vEgo == 0.0
|
||||
assert state_iq is not None
|
||||
assert actuators is not None
|
||||
assert isinstance(can_sends, list)
|
||||
assert radar_result is None
|
||||
|
||||
|
||||
def test_parameter_defaults_do_not_require_persisted_fingerprint(monkeypatch, tmp_path):
|
||||
"""A clean installation must not crash when carrotpilot-specific Params have not been written yet."""
|
||||
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
|
||||
cp = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False)
|
||||
cp_iq = CarInterface.get_params_iq(cp, CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False)
|
||||
interface = CarInterface(cp, cp_iq)
|
||||
|
||||
state, _ = interface.update([])
|
||||
assert state.vEgo == 0.0
|
||||
|
||||
|
||||
def test_classic_lfa_button_capability_survives_iq_module_removal(monkeypatch, tmp_path):
|
||||
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[0][0x391] = 8
|
||||
|
||||
cp = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False)
|
||||
cp_iq = CarInterface.get_params_iq(cp, CAR.HYUNDAI_SONATA, fingerprint, [], False, False, False)
|
||||
|
||||
assert cp_iq.flags & HyundaiFlagsIQ.HAS_LFA_BUTTON
|
||||
assert cp_iq.iqSafetyFlags & HyundaiSafetyFlagsIQ.HAS_LDA_BUTTON
|
||||
326
artifacts/package_runtime/iqdbc/car/hyundai/tests/test_radar.py
Normal file
326
artifacts/package_runtime/iqdbc/car/hyundai/tests/test_radar.py
Normal file
@@ -0,0 +1,326 @@
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, structs
|
||||
import iqdbc.car.hyundai.hyundaicanfd as hyundaicanfd
|
||||
import iqdbc.car.hyundai.radar_interface as radar_interface_module
|
||||
from iqdbc.car.hyundai.radar_interface import (
|
||||
CORNER_OBJECT_STABLE_TRACK_ID_START,
|
||||
RADAR_MSG_COUNT3,
|
||||
RADAR_MSG_COUNT4,
|
||||
RADAR_START_ADDR_CANFD3,
|
||||
CornerObjectTrackIdManager,
|
||||
RadarInterface,
|
||||
corner_object_position_valid,
|
||||
)
|
||||
from iqdbc.car.hyundai.values import CAR, HyundaiExtFlags, HyundaiFlags
|
||||
|
||||
|
||||
class TestDensoRadar:
|
||||
@staticmethod
|
||||
def parse(addr, dat):
|
||||
name = f"RADAR_TRACK_{addr:x}"
|
||||
parser = CANParser("hyundai_kia_denso_front_radar_generated", [(name, 20)], 1)
|
||||
parser.update([0, [(addr, bytes.fromhex(dat), 1)]])
|
||||
return parser.vl[name]
|
||||
|
||||
def test_active_track_signals(self):
|
||||
# Person walking toward the parked car, left of the camera center.
|
||||
track = self.parse(0x503, "bc047efcc1fe8b00")
|
||||
|
||||
assert track["LONG_DIST"] == pytest.approx(7.1875)
|
||||
assert track["LAT_DIST"] == pytest.approx(-1.625)
|
||||
assert track["REL_SPEED"] == pytest.approx(-0.734375)
|
||||
assert track["OBJECT_STATE"] == 3
|
||||
|
||||
def test_empty_track(self):
|
||||
track = self.parse(0x507, "53fff80000000081")
|
||||
|
||||
assert track["LONG_DIST"] == pytest.approx(409.55)
|
||||
assert track["LAT_DIST"] == 0
|
||||
assert track["REL_SPEED"] == 0
|
||||
assert track["OBJECT_STATE"] == 0
|
||||
|
||||
def test_long_range_lateral_distance(self):
|
||||
# Real driving sample: treating the signed field as -12 degrees would put
|
||||
# this target about 34 m sideways at 161 m. It is instead -3.0 m lateral.
|
||||
track = self.parse(0x506, "b664eafa00cd230b")
|
||||
|
||||
assert track["LONG_DIST"] == pytest.approx(161.4625)
|
||||
assert track["LAT_DIST"] == pytest.approx(-3.0)
|
||||
assert track["OBJECT_STATE"] == 3
|
||||
|
||||
def test_parser_selection_and_point_conversion(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def get_int(self, key):
|
||||
return 1 if key == "EnableRadarTracks" else 0
|
||||
|
||||
monkeypatch.setattr(radar_interface_module, "Params", FakeParams)
|
||||
cp = structs.CarParams()
|
||||
cp.carFingerprint = CAR.KIA_SORENTO
|
||||
cp.flags = 0
|
||||
cp.extFlags = HyundaiExtFlags.RADAR_GROUP4.value
|
||||
cp.radarUnavailable = False
|
||||
cp.safetyConfigs = [structs.CarParams.SafetyConfig()]
|
||||
|
||||
radar_interface = RadarInterface(cp)
|
||||
|
||||
assert radar_interface.radar_group4
|
||||
assert RADAR_MSG_COUNT4 == 8
|
||||
assert radar_interface.radar_msg_count == RADAR_MSG_COUNT4
|
||||
assert radar_interface.trigger_msg_tracks == 0x507
|
||||
|
||||
active_dat = bytes.fromhex("bc047efcc1fe8b00")
|
||||
empty_dat = bytes.fromhex("bcfff80000000081")
|
||||
packets = [(addr, active_dat if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
point = next(point for point in radar_data.points if point.trackId == 35)
|
||||
|
||||
assert point.measured
|
||||
assert point.dRel == pytest.approx(7.1875)
|
||||
assert point.yRel == pytest.approx(1.625)
|
||||
assert point.vRel == pytest.approx(-0.734375)
|
||||
assert math.isnan(point.aRel)
|
||||
|
||||
# EN: Confirm that the long-range sample survives the filter and converts
|
||||
# radar-left-negative to openpilot-left-positive coordinates.
|
||||
# KO: 장거리 샘플의 필터 통과와 레이더 좌측 음수 좌표가 openpilot 좌측
|
||||
# 양수 좌표로 변환되는지 확인함.
|
||||
long_range_dat = bytes.fromhex("b664eafa00cd230b")
|
||||
packets = [(addr, long_range_dat if addr == 0x506 else empty_dat, 1) for addr in range(0x500, 0x508)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
point = next(point for point in radar_data.points if point.trackId == 38)
|
||||
|
||||
assert point.dRel == pytest.approx(161.4625)
|
||||
assert point.yRel == pytest.approx(3.0)
|
||||
|
||||
# EN: A state-0 raw detection must not enter a stable tracked-object slot.
|
||||
# KO: 상태 0인 raw detection이 안정적인 추적 객체 슬롯에 들어오지 않음을 확인함.
|
||||
raw_detection = bytes.fromhex("d702f4fc200000e4")
|
||||
packets = [(addr, raw_detection if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
assert not radar_data.points
|
||||
|
||||
# EN: A real confirmed track beyond the former 205 m limit remains valid.
|
||||
# KO: 기존 205m 상한을 넘는 실제 확정 트랙도 유효하게 유지됨.
|
||||
confirmed_213m_track = bytes.fromhex("35854c0780f163e0")
|
||||
packets = [(addr, confirmed_213m_track if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
point = next(point for point in radar_data.points if point.trackId == 35)
|
||||
assert point.dRel == pytest.approx(213.275)
|
||||
assert point.yRel == pytest.approx(-3.75)
|
||||
|
||||
# EN: The 325 m boundary is rejected, leaving ample separation from the
|
||||
# 409.55 m empty-slot sentinel.
|
||||
# KO: 325m 경계값을 제외해 409.55m 빈 슬롯 값과 충분한 간격을 확보함.
|
||||
boundary_track = bytes.fromhex("bccb200000000300")
|
||||
packets = [(addr, boundary_track if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
assert not radar_data.points
|
||||
|
||||
# EN: The wider profile keeps a real stable track at 4.875 m, covering more
|
||||
# of the outer adjacent lane than the conservative 4.5 m profile.
|
||||
# KO: 넓어진 필터에서 4.875m의 실제 안정 트랙을 유지해 보수적인 4.5m
|
||||
# 설정보다 바깥쪽 인접 차선을 더 넓게 포함함.
|
||||
outer_lane_track = bytes.fromhex("d80b66f640000300")
|
||||
packets = [(addr, outer_lane_track if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
point = next(point for point in radar_data.points if point.trackId == 35)
|
||||
assert point.yRel == pytest.approx(4.875)
|
||||
|
||||
# EN: Tracks beyond the widened envelope are rejected as roadside clutter;
|
||||
# this payload differs only in lateral distance (-7.0 m).
|
||||
# KO: 넓어진 범위를 벗어난 트랙은 도로변 잡음으로 제외함. 이 payload는
|
||||
# 횡방향 거리(-7.0m)만 다름.
|
||||
far_side_reflection = bytes.fromhex("d80b66f200000300")
|
||||
packets = [(addr, far_side_reflection if addr == 0x503 else empty_dat, 1) for addr in range(0x500, 0x508)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
assert not radar_data.points
|
||||
|
||||
|
||||
class TestRadarGroup3:
|
||||
@staticmethod
|
||||
def parse(addr, dat):
|
||||
name = f"RADAR_TRACK_{addr:x}"
|
||||
parser = CANParser("hyundai_canfd_radar_generated", [(name, 20)], 1)
|
||||
parser.update([0, [(addr, bytes.fromhex(dat), 1)]])
|
||||
return parser.vl[name]
|
||||
|
||||
def test_group3_active_track(self):
|
||||
track = self.parse(0x406, "e1043b0f02590e692a227e16f80fe00f28fcc753a20a0000")
|
||||
|
||||
assert track["OBJECT_LENGTH"] == pytest.approx(4.4)
|
||||
assert track["LONG_DIST"] == pytest.approx(55.4)
|
||||
assert track["LAT_DIST"] == pytest.approx(-3.0)
|
||||
assert track["REL_SPEED"] == pytest.approx(4.4)
|
||||
|
||||
def test_group3_empty_track(self):
|
||||
track = self.parse(0x407, "c03d3b0000000000ff0700000000000000d0020000000000")
|
||||
|
||||
assert track["OBJECT_LENGTH"] == 0
|
||||
assert track["LONG_DIST"] == pytest.approx(204.7)
|
||||
assert track["LAT_DIST"] == 0
|
||||
assert track["REL_SPEED"] == 0
|
||||
|
||||
def test_group3_parser_selection(self, monkeypatch):
|
||||
class FakeParams:
|
||||
def get_int(self, key):
|
||||
return 1 if key == "EnableRadarTracks" else 0
|
||||
|
||||
monkeypatch.setattr(radar_interface_module, "Params", FakeParams)
|
||||
monkeypatch.setattr(hyundaicanfd, "Params", FakeParams)
|
||||
cp = structs.CarParams()
|
||||
cp.carFingerprint = next(car for car, dbc in radar_interface_module.DBC.items() if "hyundai_canfd" in dbc[Bus.pt])
|
||||
cp.flags = HyundaiFlags.CANFD.value
|
||||
cp.extFlags = HyundaiExtFlags.RADAR_GROUP3.value
|
||||
cp.radarUnavailable = False
|
||||
cp.safetyConfigs = [structs.CarParams.SafetyConfig()]
|
||||
|
||||
radar_interface = RadarInterface(cp)
|
||||
|
||||
assert radar_interface.radar_group3
|
||||
assert radar_interface.radar_start_addr == RADAR_START_ADDR_CANFD3
|
||||
assert radar_interface.radar_msg_count == RADAR_MSG_COUNT3
|
||||
assert radar_interface.trigger_msg_tracks == 0x41D
|
||||
|
||||
active_dat = bytes.fromhex("e1043b0f02590e692a227e16f80fe00f28fcc753a20a0000")
|
||||
empty_dat = bytes.fromhex("c03d3b0000000000ff0700000000000000d0020000000000")
|
||||
packets = [(addr, active_dat if addr == 0x406 else empty_dat, 1) for addr in range(0x400, 0x41E)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
point = next(point for point in radar_data.points if point.trackId == 38)
|
||||
|
||||
assert point.measured
|
||||
assert point.dRel == pytest.approx(53.1)
|
||||
assert point.yRel == pytest.approx(-3.0)
|
||||
assert point.vRel == pytest.approx(4.4)
|
||||
|
||||
|
||||
class TestCornerRadarObjectIdentity:
|
||||
@staticmethod
|
||||
def set_bits(data, start, size, value):
|
||||
for offset in range(size):
|
||||
bit = start + offset
|
||||
data[bit // 8] |= ((value >> offset) & 1) << (bit % 8)
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"dbc,msg_name,addr,age_signal,id_signal,age_start,id_start",
|
||||
(
|
||||
("hyundai_canfd_corner_radar_180_generated", "CORNER_RADAR_180_OBJECTS_180", 0x180,
|
||||
"SLOT1_AGE", "SLOT1_OBJECT_ID", 32, 44),
|
||||
("hyundai_canfd_corner_radar_235_generated", "CORNER_RADAR_235_OBJECTS_235", 0x235,
|
||||
"OBJ_AGE", "OBJ_OBJECT_ID", 32, 44),
|
||||
),
|
||||
)
|
||||
def test_object_identity_signals(self, dbc, msg_name, addr, age_signal, id_signal, age_start, id_start):
|
||||
data = bytearray(32)
|
||||
self.set_bits(data, age_start, 8, 23)
|
||||
self.set_bits(data, id_start, 7, 46)
|
||||
parser = CANParser(dbc, [(msg_name, 33)], 1)
|
||||
parser.update([0, [(addr, bytes(data), 1)]])
|
||||
|
||||
assert parser.vl[msg_name][age_signal] == 23
|
||||
assert parser.vl[msg_name][id_signal] == 46
|
||||
|
||||
def test_track_id_survives_slot_move_and_resets_with_age(self):
|
||||
manager = CornerObjectTrackIdManager()
|
||||
first_id = manager.get_track_id("corner180", object_id=108, age=240)
|
||||
|
||||
assert first_id == CORNER_OBJECT_STABLE_TRACK_ID_START
|
||||
assert manager.get_track_id("corner180", object_id=108, age=241) == first_id
|
||||
assert manager.get_track_id("corner235", object_id=108, age=241) != first_id
|
||||
assert manager.get_track_id("corner180", object_id=108, age=2) != first_id
|
||||
|
||||
def test_clipped_side_object_position_is_valid(self):
|
||||
assert corner_object_position_valid(0.0, 2.8)
|
||||
assert corner_object_position_valid(25.0, 0.2)
|
||||
assert not corner_object_position_valid(0.0, 0.0)
|
||||
assert not corner_object_position_valid(0.0, 5.0)
|
||||
|
||||
|
||||
class TestCornerRadar430CandidateFilter:
|
||||
@staticmethod
|
||||
def slot_word(distance_raw, meta13=0, b2=10, b3=2):
|
||||
return distance_raw | (meta13 << 13) | (b2 << 16) | (b3 << 24)
|
||||
|
||||
@classmethod
|
||||
def message(cls, slots):
|
||||
words = [0x010d1f40] * 7
|
||||
for slot, word in slots.items():
|
||||
words[slot - 1] = word
|
||||
|
||||
dat = bytearray(32)
|
||||
for idx, word in enumerate(words):
|
||||
dat[4 + idx * 4:8 + idx * 4] = int(word).to_bytes(4, "little")
|
||||
return bytes(dat)
|
||||
|
||||
@staticmethod
|
||||
def build_interface(monkeypatch):
|
||||
class FakeParams:
|
||||
def get_int(self, key):
|
||||
return 1 if key == "EnableCornerRadar" else 0
|
||||
|
||||
monkeypatch.setattr(radar_interface_module, "Params", FakeParams)
|
||||
monkeypatch.setattr(hyundaicanfd, "Params", FakeParams)
|
||||
cp = structs.CarParams()
|
||||
cp.carFingerprint = next(car for car, dbc in radar_interface_module.DBC.items() if "hyundai_canfd" in dbc[Bus.pt])
|
||||
cp.flags = HyundaiFlags.CANFD.value
|
||||
cp.extFlags = HyundaiExtFlags.CORNER_RADAR_OBJECTS_430.value
|
||||
cp.radarUnavailable = True
|
||||
cp.safetyConfigs = [structs.CarParams.SafetyConfig()]
|
||||
return RadarInterface(cp)
|
||||
|
||||
@staticmethod
|
||||
def update_frames(radar_interface, packets, frames=5):
|
||||
radar_data = None
|
||||
for _ in range(frames):
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
return radar_data
|
||||
|
||||
def test_430_promotes_supported_neighbor_bins(self, monkeypatch):
|
||||
radar_interface = self.build_interface(monkeypatch)
|
||||
empty = self.message({})
|
||||
supported_bins = self.message({
|
||||
6: self.slot_word(1000),
|
||||
7: self.slot_word(1004),
|
||||
})
|
||||
packets = [(addr, supported_bins if addr == 0x436 else empty, 1) for addr in range(0x430, 0x438)]
|
||||
packets += [(addr, empty, 1) for addr in range(0x440, 0x448)]
|
||||
|
||||
radar_data = self.update_frames(radar_interface, packets)
|
||||
points = {point.trackId: point for point in radar_data.points}
|
||||
|
||||
assert points[300].measured
|
||||
assert points[300].dRel == pytest.approx(50.1)
|
||||
assert points[300].yRel == pytest.approx(2.0)
|
||||
assert points[300].yvRel == 0.0
|
||||
|
||||
def test_430_expires_noncenter_inward_yvrel(self, monkeypatch):
|
||||
radar_interface = self.build_interface(monkeypatch)
|
||||
empty = self.message({})
|
||||
frame_defs = (
|
||||
(0x431, 4, 5),
|
||||
(0x433, 3, 4),
|
||||
(0x435, 2, 3),
|
||||
(0x430, 5, 6),
|
||||
(0x432, 4, 5),
|
||||
(0x434, 3, 4),
|
||||
(0x436, 2, 3),
|
||||
)
|
||||
|
||||
radar_data = None
|
||||
for addr, first_slot, second_slot in frame_defs:
|
||||
msg = self.message({
|
||||
first_slot: self.slot_word(1000),
|
||||
second_slot: self.slot_word(1004),
|
||||
})
|
||||
packets = [(a, msg if a == addr else empty, 1) for a in range(0x430, 0x438)]
|
||||
packets += [(a, empty, 1) for a in range(0x440, 0x448)]
|
||||
radar_data = radar_interface.update([0, packets])
|
||||
radar_data = self.update_frames(radar_interface, packets, frames=3)
|
||||
points = {point.trackId: point for point in radar_data.points}
|
||||
|
||||
assert points[300].measured
|
||||
assert points[300].yRel == pytest.approx(2.0)
|
||||
assert points[300].yvRel == 0.0
|
||||
1070
artifacts/package_runtime/iqdbc/car/hyundai/values.py
Normal file
1070
artifacts/package_runtime/iqdbc/car/hyundai/values.py
Normal file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user