9b1ff06bba
Maintain openpilot lateral control when the brake or gas pedals are used. Deactivation occurs only through the 'Cruise Control' button. Lots of credit goes to "pfeiferj"! Couldn't of done it without him! https: //github.com/pfeiferj/openpilot Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
214 lines
8.1 KiB
Python
214 lines
8.1 KiB
Python
import crcmod
|
|
from openpilot.selfdrive.car.hyundai.values import CAR, HyundaiFlags
|
|
|
|
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
|
|
|
|
def create_lkas11(packer, frame, CP, apply_steer, steer_req,
|
|
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
|
|
values["CF_Lkas_SysWarning"] = 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_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
|
|
|
|
if CP.carFingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
|
|
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020,
|
|
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022,
|
|
CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022,
|
|
CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED,
|
|
CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.CUSTIN_1ST_GEN):
|
|
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
|
|
values["CF_Lkas_LdwsOpt_USM"] = 2
|
|
|
|
# FcwOpt_USM 5 = Orange blinking car + lanes
|
|
# FcwOpt_USM 4 = Orange car + lanes
|
|
# FcwOpt_USM 3 = Green blinking car + lanes
|
|
# FcwOpt_USM 2 = Green car + lanes
|
|
# FcwOpt_USM 1 = White car + lanes
|
|
# FcwOpt_USM 0 = No car + lanes
|
|
values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1
|
|
|
|
# SysWarning 4 = keep hands on wheel
|
|
# SysWarning 5 = keep hands on wheel (red)
|
|
# SysWarning 6 = keep hands on wheel (red) + beep
|
|
# Note: the warning is hidden while the blinkers are on
|
|
values["CF_Lkas_SysWarning"] = 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
|
|
|
|
dat = packer.make_can_msg("LKAS11", 0, values)[2]
|
|
|
|
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, enabled, lat_active, hda_set_speed=0):
|
|
values = {
|
|
"LFA_Icon_State": 2 if lat_active else 0,
|
|
"HDA_Active": 1 if hda_set_speed else 0,
|
|
"HDA_Icon_State": 2 if hda_set_speed else 0,
|
|
"HDA_VSetReq": hda_set_speed,
|
|
}
|
|
return packer.make_can_msg("LFAHDA_MFC", 0, values)
|
|
|
|
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, cruise_available):
|
|
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,
|
|
"ObjValid": 1, # close lead makes controls tighter
|
|
"ACC_ObjStatus": 1, # close lead makes controls tighter
|
|
"ACC_ObjLatPos": 0,
|
|
"ACC_ObjRelSpd": 0,
|
|
"ACC_ObjDist": 1, # close lead makes controls tighter
|
|
}
|
|
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
|
|
|
|
scc12_values = {
|
|
"ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
|
|
"StopReq": 1 if stopping else 0,
|
|
"aReqRaw": accel,
|
|
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
|
|
"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)[2]
|
|
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": 0.0, # stock usually is 0 but sometimes uses higher values
|
|
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
|
|
"JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
|
|
"JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
|
|
"ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
|
"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
|
|
}
|
|
commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
|
|
|
|
# Only send FCA11 on cars where it exists on the bus
|
|
if 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)[2]
|
|
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):
|
|
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
|
|
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)
|