88 lines
2.9 KiB
Python
88 lines
2.9 KiB
Python
import crcmod
|
|
from selfdrive.car.hyundai.values import CAR, CHECKSUM
|
|
|
|
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
|
|
|
|
|
|
def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
|
|
lkas11, sys_warning, sys_state, enabled,
|
|
left_lane, right_lane,
|
|
left_lane_depart, right_lane_depart):
|
|
values = lkas11
|
|
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_MsgCount"] = frame % 0x10
|
|
|
|
if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV]:
|
|
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
|
|
|
|
elif car_fingerprint == CAR.HYUNDAI_GENESIS:
|
|
# This field is actually LdwsActivemode
|
|
# Genesis and Optima fault when forwarding while engaged
|
|
values["CF_Lkas_LdwsActivemode"] = 2
|
|
elif car_fingerprint == CAR.KIA_OPTIMA:
|
|
values["CF_Lkas_LdwsActivemode"] = 0
|
|
|
|
dat = packer.make_can_msg("LKAS11", 0, values)[2]
|
|
|
|
if car_fingerprint in CHECKSUM["crc8"]:
|
|
# CRC Checksum as seen on 2019 Hyundai Santa Fe
|
|
dat = dat[:6] + dat[7:8]
|
|
checksum = hyundai_checksum(dat)
|
|
elif car_fingerprint in 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):
|
|
values = clu11
|
|
values["CF_Clu_CruiseSwState"] = button
|
|
values["CF_Clu_AliveCnt1"] = frame % 0x10
|
|
return packer.make_can_msg("CLU11", 0, values)
|
|
|
|
|
|
def create_lfa_mfa(packer, frame, enabled):
|
|
values = {
|
|
"ACTIVE": enabled,
|
|
}
|
|
|
|
# ACTIVE 1 = Green steering wheel icon
|
|
|
|
# LFA_USM 2 & 3 = LFA cancelled, fast loud beeping
|
|
# LFA_USM 0 & 1 = No mesage
|
|
|
|
# LFA_SysWarning 1 = "Switching to HDA", short beep
|
|
# LFA_SysWarning 2 = "Switching to Smart Cruise control", short beep
|
|
# LFA_SysWarning 3 = LFA error
|
|
|
|
# ACTIVE2: nothing
|
|
# HDA_USM: nothing
|
|
|
|
return packer.make_can_msg("LFAHDA_MFC", 0, values)
|