Skip to content

차선변경 디버그 추가 #241

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
thftgr wants to merge 12 commits into from
Closed

차선변경 디버그 추가 #241

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
9fb289b
정지 옆두부 활성화, 차선변경 시험.
Jan 9, 2026
5c9bf3f
차선변경
Jan 9, 2026
6eb5c42
Merge remote-tracking branch 'origin/n-sc' into m-wmi6-x-2
Jan 9, 2026
d2b4461
차선변경 표시 테스트
Jan 9, 2026
2669dc3
fix
Jan 9, 2026
40f2c33
fix
Jan 9, 2026
4991955
차선변경 테스트
Jan 9, 2026
778b45e
차선변경 테스트
Jan 9, 2026
8264434
조향 동기화 차선커브
Jan 9, 2026
1aef1fc
후측방 먼곳 감지의경우 점멸
Jan 9, 2026
40a8ef7
fix
Jan 9, 2026
32c3bf1
fix2
thftgr Jan 9, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 9 additions & 9 deletions opendbc_repo/opendbc/car/hyundai/carcontroller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -164,7 +164,7 @@ def update(self, CC, CS, now_nanos):
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"))

Expand All @@ -186,7 +186,7 @@ def update(self, CC, CS, now_nanos):
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
Expand All @@ -198,7 +198,7 @@ def update(self, CC, CS, now_nanos):
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,
#apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw,
# CS.out.steeringAngleDeg, CC.latActive, self.params.ANGLE_LIMITS)

MAX_LAT_ACCEL = 8.0
Expand All @@ -213,7 +213,7 @@ def update(self, CC, CS, now_nanos):
rate_deg_s = calc_rate_limit_by_lat_accel(self.apply_angle_last, CS.out.vEgoRaw, self.CP.wheelbase, MAX_LAT_ACCEL, MAX_RATE_LOW, MAX_RATE_HIGH)
if not same_dir:
rate_deg_s = min(rate_deg_s * UNWIND_SCALE, UNWIND_MAX)

rate_deg_per_tick = rate_deg_s * DT_CTRL
apply_angle = np.clip(actuators.steeringAngleDeg,
self.apply_angle_last - rate_deg_per_tick,
Expand Down Expand Up @@ -276,7 +276,7 @@ def update(self, CC, CS, now_nanos):

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: # 과속카메라 감속시작
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
Expand Down Expand Up @@ -346,7 +346,7 @@ def update(self, CC, CS, now_nanos):
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))
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, self.canfd_debug))
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.CP, self.packer, self.CAN, self.frame))
else:
Expand All @@ -367,7 +367,7 @@ def update(self, CC, CS, now_nanos):
can_sends.extend(hyundaicanfd.forward_button_message(self.packer, self.CAN, self.frame, CS, send_button, self.MainMode_ACC_trigger, self.LFA_trigger))
else:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))

else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_torque, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
Expand All @@ -386,7 +386,7 @@ def update(self, CC, CS, now_nanos):
#jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
if camera_scc:

can_sends.extend(hyundaican.create_acc_commands_scc(self.packer, CC.enabled, accel, self.hyundai_jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, casper_opt, CS, self.soft_hold_mode))
Expand Down Expand Up @@ -611,7 +611,7 @@ def check_carrot_cruise(self, CC, CS, hud_control, stopping, accel, a_target):
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
Expand Down
16 changes: 9 additions & 7 deletions opendbc_repo/opendbc/car/hyundai/carstate.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,8 @@ def __init__(self, CP):
self.LFA_ICON = 0
self.paddle_button_prev = 0

self.lane_changing = 0 # 0 ~ 4

self.lf_distance = 0
self.rf_distance = 0
self.lr_distance = 0
Expand All @@ -138,7 +140,7 @@ def __init__(self, CP):
self.SCC14 = True if 905 in fingerprints[bus_cruise] else False
self.FCA11 = False
self.FCA11_bus = Bus.cam

self.HAS_LFA_BUTTON = True if 913 in fingerprints[0] else False
self.CRUISE_BUTTON_ALT = True if 1007 in fingerprints[0] else False

Expand All @@ -163,7 +165,7 @@ def __init__(self, CP):

self.cp_bsm = None
self.time_zone = "UTC"

self.controls_ready_count = 0

def update(self, can_parsers) -> structs.CarState:
Expand Down Expand Up @@ -459,7 +461,7 @@ def update_canfd(self, can_parsers) -> structs.CarState:
ret.steeringAngleDeg = cp.vl["MDPS"]["STEERING_ANGLE_2"] * -1
else:
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1

ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
Expand All @@ -470,7 +472,7 @@ def update_canfd(self, can_parsers) -> structs.CarState:
if self.STEER_TOUCH_2AF:
self.steer_touch_info = cp.vl["STEER_TOUCH_2AF"]

blinkers_info = cp.vl["BLINKERS"]
blinkers_info = cp.vl["BLINKERS"]
left_blinker_lamp = blinkers_info["LEFT_LAMP"] or blinkers_info["LEFT_LAMP_ALT"]
right_blinker_lamp = blinkers_info["RIGHT_LAMP"] or blinkers_info["RIGHT_LAMP_ALT"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, left_blinker_lamp, right_blinker_lamp)
Expand All @@ -497,7 +499,7 @@ def update_canfd(self, can_parsers) -> structs.CarState:
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
Expand All @@ -524,7 +526,7 @@ def update_canfd(self, can_parsers) -> structs.CarState:

if self.LFAHDA_CLUSTER:
self.lfahda_cluster_info = cp_cam.vl["LFAHDA_CLUSTER"]

corner = False
self.adrv_info_161 = cp_cam.vl["ADRV_0x161"] if self.CCNC_0x161 else None
self.adrv_info_162 = cp_cam.vl["CCNC_0x162"] if self.CCNC_0x162 else None
Expand Down Expand Up @@ -561,7 +563,7 @@ def update_canfd(self, can_parsers) -> structs.CarState:

self.new_msg_4b4 = cp.vl["NEW_MSG_4B4"] if self.NEW_MSG_4B4 else None
self.tcs_info_373 = cp.vl["TCS"]

ret.gearStep = cp.vl["GEAR"]["GEAR_STEP"] if self.GEAR else 0
if 1 <= ret.gearStep <= 8 and ret.gearShifter == GearShifter.unknown:
ret.gearShifter = GearShifter.drive
Expand Down
83 changes: 73 additions & 10 deletions opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active,
values["HAS_LANE_SAFETY"] = 0
values["LKA_ACTIVE"] = 0 # NEW_SIGNAL_1

values["DampingGain"] = 0 if lat_active else 100
values["DampingGain"] = 0 if lat_active else 100
#values["VALUE63"] = 0

#values["VALUE82_SET256"] = 0
Expand Down Expand Up @@ -274,7 +274,7 @@ def create_lfahda_cluster(packer, CS, CAN, long_active, lat_active):
values["HDA_CntrlModSta"] = 2 if long_active else 0
values["HDA_LFA_SymSta"] = 2 if lat_active else 0

#
#
else:
return []
return [packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)]
Expand All @@ -289,7 +289,7 @@ def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, g
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

Expand Down Expand Up @@ -346,7 +346,7 @@ def create_acc_control_scc2(packer, CAN, enabled, accel_last, accel, stopping, g
# AccelLimitBandUpper, Lower
values["SysFailState"] = 0 # 1: Performance degredation, 2: system temporairy unavailble, 3: SCC Service required , 눈이 묻어 레이더오류시... 2가 됨. 이때 가속을 안함...

values["AccelLimitBandUpper"] = 0.0 # 이값이 1.26일때 가속을 안하는 증상이 보임..
values["AccelLimitBandUpper"] = 0.0 # 이값이 1.26일때 가속을 안하는 증상이 보임..
values["AccelLimitBandLower"] = 0.0

values["ZEROS_7"] = 1
Expand Down Expand Up @@ -457,14 +457,14 @@ def forward_button_message(packer, CAN, frame, CS, cruise_button, MainMode_ACC_t
ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))
return ret

def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle, left_lane_warning, right_lane_warning, enable_corner_radar):
def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle, left_lane_warning, right_lane_warning, enable_corner_radar,canfd_debug):
ret = []

if CP.flags & HyundaiFlags.CAMERA_SCC.value:
HDA_CntrlModSta = 0
if CS.lfahda_cluster_info is not None:
HDA_CntrlModSta = CS.lfahda_cluster_info["HDA_CntrlModSta"]

if frame % 2 == 0:
if CS.adrv_info_160 is not None:
values = copy.copy(CS.adrv_info_160)
Expand All @@ -475,7 +475,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
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)
values = copy.copy(CS.cruise_buttons_msg)
if CS.lfahda_cluster_info["HDA_LFA_SymSta"] == 0 and 0 < frame % 200 < 12:
values["LFA_BTN"] = 1
else:
Expand All @@ -488,7 +488,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
values["ADAPTIVE_CRUISE_MAIN_BTN"] = 1
else:
values["ADAPTIVE_CRUISE_MAIN_BTN"] = 0

ret.append(packer.make_can_msg(CS.cruise_btns_msg_canfd, CAN.CAM, values))


Expand Down Expand Up @@ -558,7 +558,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
values["LANELINE_CURVATURE"] = (min(abs(curvature), 15) + (-1 if curvature < 0 else 0)) if lat_active else 0
values["LANELINE_CURVATURE_DIRECTION"] = 1 if curvature < 0 and lat_active else 0

# lane_color = 6 if lat_active else 2
# lane_color = 6 if lat_active else 2
lane_color = 2 # 6: green, 2: white, 4: yellow
if hud_control.leftLaneDepart:
values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1
Expand Down Expand Up @@ -594,6 +594,69 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
if CC.rightBlinker:
#values['RIGHT_BLINK_HOLD'] = 1
pass

if canfd_debug > 0:
# 조향 동기화 차선커브
curvature = round(CS.out.steeringAngleDeg / 3)
values["LANELINE_CURVATURE"] = max(0,min(abs(curvature), 15) + (-1 if curvature < 0 else 0))
values["LANELINE_CURVATURE_DIRECTION"] = 1 if curvature < 0 else 0

if canfd_debug > 0:
# 정차, 저속시 클러스터에 보이게.
if values['LF_DETECT'] == 4 and values['LF_DETECT_DISTANCE'] != 0: values['LF_DETECT'] = 2
if values['RF_DETECT'] == 4 and values['RF_DETECT_DISTANCE'] != 0: values['RF_DETECT'] = 2
if values['LR_DETECT'] == 4 and values['LR_DETECT_DISTANCE'] != 0: values['LR_DETECT'] = 2
if values['RR_DETECT'] == 4 and values['RR_DETECT_DISTANCE'] != 0: values['RR_DETECT'] = 2

if values['LR_DETECT_DISTANCE'] > 14:
d = min(values['LR_DETECT_DISTANCE'], 100.0)
interval = int(1 + 99 * (d / 100.0)) # 1..100 frames
blink = (frame // interval) & 1
values['LR_DETECT'] = 2 - blink # 멀수록 천천히 점멸
values['LR_DETECT_DISTANCE'] = 14

if values['RR_DETECT_DISTANCE'] > 14:
d = min(values['RR_DETECT_DISTANCE'], 100.0)
interval = int(1 + 99 * (d / 100.0)) # 1..100 frames
blink = (frame // interval) & 1
values['RR_DETECT'] = 2 - blink # 멀수록 천천히 점멸
values['RR_DETECT_DISTANCE'] = 14

if canfd_debug == 1:
# print(f"hud_control= {hud_control.modelDesire}")
if hud_control.modelDesire == 1: # # 좌회전
values['LANE_CHANGING'] = 1 # 왼쪽 화살표
values["LANELINE_CURVATURE"] = 15 # 커브 최대
values["LANELINE_CURVATURE_DIRECTION"] = 0 # 왼쪽으로

elif hud_control.modelDesire == 2: # 우회전
values['LANE_CHANGING'] = 2 # 오른쪽 화살표
values["LANELINE_CURVATURE"] = 15 # 차선커브 최대로
values["LANELINE_CURVATURE_DIRECTION"] = 1 # 오른쪽으로

elif hud_control.modelDesire == 3: # 좌차선변경
values['LANE_CHANGING'] = 3 # 왼쪽 화살표 + 바닥

elif hud_control.modelDesire == 4: # 우차선변경
values['LANE_CHANGING'] = 4 # 오른쪽 화살표 + 바닥

if canfd_debug == 2:
#차선변경중 상태 표시
llpos = values["LANELINE_LEFT_POSITION"] # 0~15~30
lrpos = values["LANELINE_RIGHT_POSITION"] # 0~15~30
lc = CS.lane_changing
if lc == 0: # 차선변경중이 아니면
if llpos < 13: CS.lane_changing = 1 # 왼쪽 화살표
if lrpos < 13: CS.lane_changing = 2 # 오른쪽 화살표
elif llpos == 15 or lrpos == 15: # 중앙이면
CS.lane_changing = 0
else:
if lc == 1 and llpos < 12: CS.lane_changing = 3 # 왼쪽 화살표 + 바닥
if lc == 2 and lrpos < 12: CS.lane_changing = 4 # 오른쪽 화살표 + 바닥

values['LANE_CHANGING'] = CS.lane_changing


ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))

if CS.adrv_info_162 is not None:
Expand Down Expand Up @@ -706,7 +769,7 @@ def alt_cruise_buttons(packer, CP, CAN, buttons, cruise_btns_msg, cnt):
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)):
Expand Down
8 changes: 7 additions & 1 deletion opendbc_repo/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -506,7 +506,7 @@ BO_ 480 LFAHDA_CLUSTER: 16 FR_CMR
SG_ HDA_LFA_WrnSnd : 49|2@1+ (1,0) [0|0] "" Dummy
SG_ HDA_InfoPUDis1 : 51|3@1+ (1,0) [0|7] "" CLU,CGW
SG_ HDA_TDMRMDclReq : 54|2@1+ (1,0) [0|3] "" Dummy

BO_ 490 ADRV_0x1ea: 32 ADRV
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
Expand Down Expand Up @@ -1019,9 +1019,15 @@ VAL_ 480 HDA_LFA_SymSta 0 "Off" 1 "Gray" 2 "Green" 3 "Green blink" ;
VAL_ 480 HDA_LFA_WrnSnd 0 "Off " 1 "Additional Warning Sound" 2 "Reserved" 3 "Error indicator" ;
VAL_ 480 HDA_InfoPUDis1 0 "No Pop-up" 1 "System Automatic off (LFA)" 2 "System Automatic off (HDA)" 3 "Reserved" 4 "Reserved" 5 "Reserved" 6 "Not Used" 7 "Error Indicator" ;
VAL_ 480 HDA_TDMRMDclReq 0 "Not automatically deactivated state of LFA (default)" 1 "Automatically deactivated state of LFA" 2 "Reserved" 3 "Reserved" ;
VAL_ 490 HDA_MODE2 1 "Lane change icon gray" 2 "lane green + lane change icon green" 3 "lane green + lane change icon green (both blinking)" 4 "lane white + lane change icon white (both blinking rapidly)" 5 "Lane Change Assist system check warning";
VAL_ 490 LANE_LEFT 0 "IDLE" 1 "ON_SOURCE" 2 "ON_TARGET";
VAL_ 490 LANE_RIGHT 0 "IDLE" 1 "ON_SOURCE" 2 "ON_TARGET";
VAL_ 490 LANE_CHANGING 0 "IDLE" 1 "LEFT_CHECK" 3 "LEFT_CHANGING" 2 "RIGHT_CHECK" 4 "RIGHT_CHANGING";
VAL_ 490 LF_DETECT 0 "none" 1 "gray" 2 "white" 3 "white" 4 "hide";
VAL_ 490 RF_DETECT 0 "none" 1 "gray" 2 "white" 3 "white" 4 "hide";
VAL_ 490 LR_DETECT 0 "none" 1 "gray" 2 "white" 3 "white" 4 "hide";
VAL_ 490 RR_DETECT 0 "none" 1 "gray" 2 "white" 3 "white" 4 "hide";
VAL_ 490 AUTOLANECHANGE_MSG 1 "Check surrounding conditions" 2 "Operating conditions not met" 3 "Analyzing driving lane" 4 "Sharp curve ahead" 5 "Current lane is too narrow" 6 "Not an operational section" 7 "Hazard lights are on" 8 "Vehicle speed is too low" 9 "Please hold the steering wheel" 10 "Not an operable lane" 11 "Steering input detected" 12 "Press the OK button to activate lane change assist";
VAL_ 490 LANELINE_CURVATURE_DIRECTION 0 "LEFT" 1 "RIGHT";
VAL_ 676 LEFT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence";
VAL_ 676 RIGHT_LANE_LINE 0 "Not Detected" 1 "Low Confidence" 2 "Medium Confidence" 3 "High Confidence";
Expand Down
Loading