2023-09-27 15:45:31 -07:00
import crcmod
2025-01-29 09:09:58 +00:00
from opendbc . car . hyundai . values import CAR , HyundaiFlags
2023-09-27 15:45:31 -07:00
hyundai_checksum = crcmod . mkCrcFun ( 0x11D , initCrc = 0xFD , rev = False , xorOut = 0xdf )
2024-06-11 01:36:40 +00:00
def create_lkas11 ( packer , frame , CP , apply_steer , steer_req ,
2023-09-27 15:45:31 -07:00
torque_fault , lkas11 , sys_warning , sys_state , enabled ,
left_lane , right_lane ,
left_lane_depart , right_lane_depart ) :
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
2024-09-03 16:09:00 +09:00
values [ " CF_Lkas_SysWarning " ] = 0 # 3 if sys_warning else 0
2023-09-27 15:45:31 -07:00
values [ " CF_Lkas_LdwsLHWarning " ] = left_lane_depart
values [ " CF_Lkas_LdwsRHWarning " ] = right_lane_depart
values [ " CR_Lkas_StrToqReq " ] = apply_steer
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
2024-09-03 16:09:00 +09:00
if CP . flags & HyundaiFlags . SEND_LFA . value :
2023-09-27 15:45:31 -07:00
values [ " CF_Lkas_LdwsActivemode " ] = int ( left_lane ) + ( int ( right_lane ) << 1 )
values [ " CF_Lkas_LdwsOpt_USM " ] = 2
# FcwOpt_USM 5 = Orange blinking car + lanes
# FcwOpt_USM 4 = Orange car + lanes
# FcwOpt_USM 3 = Green blinking car + lanes
# FcwOpt_USM 2 = Green car + lanes
# FcwOpt_USM 1 = White car + lanes
# FcwOpt_USM 0 = No car + lanes
values [ " CF_Lkas_FcwOpt_USM " ] = 2 if enabled else 1
# SysWarning 4 = keep hands on wheel
# SysWarning 5 = keep hands on wheel (red)
# SysWarning 6 = keep hands on wheel (red) + beep
# Note: the warning is hidden while the blinkers are on
2024-09-03 16:09:00 +09:00
values [ " CF_Lkas_SysWarning " ] = 0 #4 if sys_warning else 0
2023-09-27 15:45:31 -07:00
# Likely cars lacking the ability to show individual lane lines in the dash
2024-06-11 01:36:40 +00:00
elif CP . carFingerprint in ( CAR . KIA_OPTIMA_G4 , CAR . KIA_OPTIMA_G4_FL ) :
2023-09-27 15:45:31 -07:00
# 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
2024-06-11 01:36:40 +00:00
elif CP . carFingerprint == CAR . HYUNDAI_GENESIS :
2023-09-27 15:45:31 -07:00
# This field is actually LdwsActivemode
# Genesis and Optima fault when forwarding while engaged
values [ " CF_Lkas_LdwsActivemode " ] = 2
2025-01-29 09:09:58 +00:00
dat = packer . make_can_msg ( " LKAS11 " , 0 , values ) [ 1 ]
2023-09-27 15:45:31 -07:00
2024-06-11 01:36:40 +00:00
if CP . flags & HyundaiFlags . CHECKSUM_CRC8 :
2023-09-27 15:45:31 -07:00
# CRC Checksum as seen on 2019 Hyundai Santa Fe
dat = dat [ : 6 ] + dat [ 7 : 8 ]
checksum = hyundai_checksum ( dat )
2024-06-11 01:36:40 +00:00
elif CP . flags & HyundaiFlags . CHECKSUM_6B :
2023-09-27 15:45:31 -07:00
# 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 )
2024-06-11 01:36:40 +00:00
def create_clu11 ( packer , frame , clu11 , button , CP ) :
2023-09-27 15:45:31 -07:00
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
2024-06-11 01:36:40 +00:00
bus = 2 if CP . flags & HyundaiFlags . CAMERA_SCC else 0
2023-09-27 15:45:31 -07:00
return packer . make_can_msg ( " CLU11 " , bus , values )
2024-09-03 16:09:00 +09:00
def create_lfahda_mfc ( packer , CC , blinking_signal ) :
activeCarrot = CC . hudControl . activeCarrot
2023-09-27 15:45:31 -07:00
values = {
2024-09-03 16:09:00 +09:00
" 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,
2023-09-27 15:45:31 -07:00
}
return packer . make_can_msg ( " LFAHDA_MFC " , 0 , values )
2024-09-03 16:09:00 +09:00
def create_acc_commands_scc ( packer , enabled , accel , jerk , idx , hud_control , set_speed , stopping , long_override , use_fca , CS , soft_hold_mode ) :
from opendbc . car . hyundai . carcontroller import HyundaiJerk
cruise_available = CS . out . cruiseState . available
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 = [ ]
if CS . scc11 is not None :
values = 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 = 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 = 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 ) )
# Only send FCA11 on cars where it exists on the bus
if False : #use_fca:
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
" CR_FCA_Alive " : idx % 0xF ,
" PAINT1_Status " : 1 ,
" FCA_DrvSetStatus " : 1 ,
" FCA_Status " : 1 , # AEB disabled
}
fca11_dat = packer . make_can_msg ( " FCA11 " , 0 , fca11_values ) [ 1 ]
fca11_values [ " CR_FCA_ChkSum " ] = hyundai_checksum ( fca11_dat [ : 7 ] )
commands . append ( packer . make_can_msg ( " FCA11 " , 0 , fca11_values ) )
return commands
def create_acc_opt_copy ( CS , packer ) :
return packer . make_can_msg ( " SCC13 " , 0 , CS . scc13 )
def create_acc_commands ( packer , enabled , accel , jerk , idx , hud_control , set_speed , stopping , long_override , use_fca , CP , CS , soft_hold_mode ) :
from opendbc . car . hyundai . carcontroller import HyundaiJerk
cruise_available = CS . out . cruiseState . available
soft_hold_active = CS . softHoldActive
soft_hold_info = soft_hold_active > 1 and enabled
#soft_hold_mode = 2 ## some cars can't enable while braking
long_enabled = enabled or ( soft_hold_active > 0 and soft_hold_mode == 2 )
stop_req = 1 if stopping or ( soft_hold_active > 0 and soft_hold_mode == 2 ) else 0
d = hud_control . leadDistance
objGap = 0 if d == 0 else 2 if d < 25 else 3 if d < 40 else 4 if d < 70 else 5
objGap2 = 0 if objGap == 0 else 2 if hud_control . leadRelSpeed < - 0.2 else 1
if long_enabled :
scc12_acc_mode = 2 if long_override else 1
scc14_acc_mode = 2 if long_override else 1
if CS . out . brakeHoldActive :
scc12_acc_mode = 0
scc14_acc_mode = 4
elif CS . out . brakePressed :
scc12_acc_mode = 1
scc14_acc_mode = 1
else :
scc12_acc_mode = 0
scc14_acc_mode = 4
warning_front = False
2023-09-27 15:45:31 -07:00
commands = [ ]
scc11_values = {
2024-09-03 16:09:00 +09:00
" MainMode_ACC " : 1 if cruise_available else 0 ,
2024-06-11 01:36:40 +00:00
" TauGapSet " : hud_control . leadDistanceBars ,
2023-09-27 15:45:31 -07:00
" VSetDis " : set_speed if enabled else 0 ,
" AliveCounterACC " : idx % 0x10 ,
2024-09-03 16:09:00 +09:00
" 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
2023-09-27 15:45:31 -07:00
" ACC_ObjLatPos " : 0 ,
2024-09-03 16:09:00 +09:00
" ACC_ObjRelSpd " : hud_control . leadRelSpeed ,
" ACC_ObjDist " : int ( hud_control . leadDistance ) , # close lead makes controls tighter
" DriverAlertDisplay " : 0 ,
2023-09-27 15:45:31 -07:00
}
commands . append ( packer . make_can_msg ( " SCC11 " , 0 , scc11_values ) )
scc12_values = {
2024-09-03 16:09:00 +09:00
" ACCMode " : scc12_acc_mode ,
" StopReq " : stop_req ,
" aReqRaw " : 0 if stop_req > 0 else accel ,
2023-09-27 15:45:31 -07:00
" aReqValue " : accel , # stock ramps up and down respecting jerk limit until it reaches aReqRaw
2024-09-03 16:09:00 +09:00
#"DESIRED_DIST": CS.out.vEgo * 1.0 + 4.0,
2023-09-27 15:45:31 -07:00
" CR_VSM_Alive " : idx % 0xF ,
}
2023-11-17 23:53:40 +00:00
# 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
2025-01-29 09:09:58 +00:00
scc12_dat = packer . make_can_msg ( " SCC12 " , 0 , scc12_values ) [ 1 ]
2023-09-27 15:45:31 -07:00
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 = {
2024-09-03 16:09:00 +09:00
" 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 ,
2023-09-27 15:45:31 -07:00
}
commands . append ( packer . make_can_msg ( " SCC14 " , 0 , scc14_values ) )
2023-11-17 23:53:40 +00:00
# Only send FCA11 on cars where it exists on the bus
2025-01-29 09:09:58 +00:00
# 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 ) :
2023-11-17 23:53:40 +00:00
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
" CR_FCA_Alive " : idx % 0xF ,
" PAINT1_Status " : 1 ,
" FCA_DrvSetStatus " : 1 ,
" FCA_Status " : 1 , # AEB disabled
}
2025-01-29 09:09:58 +00:00
fca11_dat = packer . make_can_msg ( " FCA11 " , 0 , fca11_values ) [ 1 ]
2023-11-17 23:53:40 +00:00
fca11_values [ " CR_FCA_ChkSum " ] = hyundai_checksum ( fca11_dat [ : 7 ] )
commands . append ( packer . make_can_msg ( " FCA11 " , 0 , fca11_values ) )
2023-09-27 15:45:31 -07:00
return commands
2025-01-29 09:09:58 +00:00
def create_acc_opt ( packer , CP ) :
2023-09-27 15:45:31 -07:00
commands = [ ]
scc13_values = {
" SCCDrvModeRValue " : 2 ,
" SCC_Equip " : 1 ,
" Lead_Veh_Dep_Alert_USM " : 2 ,
}
commands . append ( packer . make_can_msg ( " SCC13 " , 0 , scc13_values ) )
2023-11-17 23:53:40 +00:00
# TODO: this needs to be detected and conditionally sent on unsupported long cars
2025-01-29 09:09:58 +00:00
# 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 ) )
2023-09-27 15:45:31 -07:00
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 )
2024-09-03 16:09:00 +09:00
def create_clu11_button ( packer , frame , clu11 , button , CP ) :
values = clu11
values [ " CF_Clu_CruiseSwState " ] = button
#values["CF_Clu_AliveCnt1"] = frame % 0x10
values [ " CF_Clu_AliveCnt1 " ] = ( values [ " CF_Clu_AliveCnt1 " ] + 1 ) % 0x10
# send buttons to camera on camera-scc based cars
bus = 2 if CP . flags & HyundaiFlags . CAMERA_SCC else 0
return packer . make_can_msg ( " CLU11 " , bus , values )
def create_mdps12 ( packer , frame , mdps12 ) :
values = mdps12
values [ " CF_Mdps_ToiActive " ] = 0
values [ " CF_Mdps_ToiUnavail " ] = 1
values [ " CF_Mdps_MsgCount2 " ] = frame % 0x100
values [ " CF_Mdps_Chksum2 " ] = 0
dat = packer . make_can_msg ( " MDPS12 " , 2 , values ) [ 1 ]
checksum = sum ( dat ) % 256
values [ " CF_Mdps_Chksum2 " ] = checksum
return packer . make_can_msg ( " MDPS12 " , 2 , values )