84 lines
3.4 KiB
Python
84 lines
3.4 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
|
|
from enum import StrEnum
|
|
|
|
from iqdbc.car import Bus, structs
|
|
from iqdbc.can.parser import CANParser
|
|
from iqdbc.car.hyundai.values import HyundaiFlags
|
|
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
|
|
|
|
|
class CarStateExt:
|
|
def __init__(self, CP, CP_IQ):
|
|
self.CP = CP
|
|
self.CP_IQ = CP_IQ
|
|
|
|
self.aBasis = 0.0
|
|
|
|
def update_speed_limit(self, cp, cp_cam) -> float:
|
|
speed_limit = 0
|
|
|
|
if self.CP.flags & HyundaiFlags.CANFD:
|
|
if self.CP_IQ.flags & HyundaiFlagsIQ.SPEED_LIMIT_AVAILABLE:
|
|
bus = cp if self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING else cp_cam
|
|
speed_limit = bus.vl["FR_CMR_02_100ms"]["ISLW_SpdCluMainDis"]
|
|
else:
|
|
nav, cam = 0, 0
|
|
if self.CP_IQ.flags & HyundaiFlagsIQ.SPEED_LIMIT_AVAILABLE:
|
|
nav = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
|
|
if self.CP_IQ.flags & HyundaiFlagsIQ.HAS_LKAS12:
|
|
cam = cp_cam.vl["LKAS12"]["CF_Lkas_TsrSpeed_Display_Clu"]
|
|
|
|
speed_limit = cam if cam not in (0, 255) else nav
|
|
|
|
if speed_limit in (0, 255):
|
|
speed_limit = 0
|
|
|
|
return speed_limit
|
|
|
|
def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser], speed_conv: float) -> None:
|
|
cp = can_parsers[Bus.pt]
|
|
cp_cam = can_parsers[Bus.cam]
|
|
|
|
self.aBasis = cp.vl["TCS13"]["aBasis"]
|
|
|
|
if self.CP_IQ.flags & HyundaiFlagsIQ.NON_SCC:
|
|
cruise_msg = "LABEL11" if self.CP.flags & HyundaiFlags.EV else \
|
|
"E_CRUISE_CONTROL" if self.CP.flags & HyundaiFlags.HYBRID else \
|
|
"EMS16"
|
|
cruise_available_sig = "CC_React" if self.CP.flags & HyundaiFlags.EV else "CRUISE_LAMP_M"
|
|
cruise_enabled_sig = "CC_ACT" if self.CP.flags & HyundaiFlags.EV else "CRUISE_LAMP_S"
|
|
cruise_speed_msg = "E_EMS11" if self.CP.flags & HyundaiFlags.EV else \
|
|
"ELECT_GEAR" if self.CP.flags & HyundaiFlags.HYBRID else \
|
|
"LVR12"
|
|
cruise_speed_sig = "Cruise_Limit_Target" if self.CP.flags & HyundaiFlags.EV else \
|
|
"SLC_SET_SPEED" if self.CP.flags & HyundaiFlags.HYBRID else \
|
|
"CF_Lvr_CruiseSet"
|
|
ret.cruiseState.available = cp.vl[cruise_msg][cruise_available_sig] != 0
|
|
ret.cruiseState.enabled = cp.vl[cruise_msg][cruise_enabled_sig] != 0
|
|
ret.cruiseState.speed = cp.vl[cruise_speed_msg][cruise_speed_sig] * speed_conv
|
|
ret.cruiseState.standstill = False
|
|
ret.cruiseState.nonAdaptive = False
|
|
|
|
if not self.CP_IQ.flags & HyundaiFlagsIQ.NON_SCC_NO_FCA:
|
|
cp_cruise = cp if self.CP_IQ.flags & HyundaiFlagsIQ.NON_SCC_RADAR_FCA else cp_cam
|
|
|
|
aeb_src = "FCA11"
|
|
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
|
|
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src]["FCA_CmdAct"] != 0
|
|
ret.stockFcw = aeb_warning and not aeb_braking
|
|
ret.stockAeb = aeb_warning and aeb_braking
|
|
|
|
ret_iq.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_conv
|
|
|
|
def update_canfd_ext(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser],
|
|
speed_factor: float) -> None:
|
|
cp = can_parsers[Bus.pt]
|
|
cp_cam = can_parsers[Bus.cam]
|
|
|
|
self.aBasis = cp.vl["TCS"]["aBasis"]
|
|
|
|
ret_iq.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_factor
|