version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
87 lines
3.5 KiB
Python
87 lines
3.5 KiB
Python
"""
|
|
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
|
|
|
This file is part of sunnypilot and is licensed under the MIT License.
|
|
See the LICENSE.md file in the root directory for more details.
|
|
"""
|
|
|
|
from enum import StrEnum
|
|
|
|
from opendbc.car import Bus, structs
|
|
from opendbc.can.parser import CANParser
|
|
from opendbc.car.hyundai.values import HyundaiFlags
|
|
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
|
|
|
|
|
class CarStateExt:
|
|
def __init__(self, CP, CP_SP):
|
|
self.CP = CP
|
|
self.CP_SP = CP_SP
|
|
|
|
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_SP.flags & HyundaiFlagsSP.SPEED_LIMIT_AVAILABLE:
|
|
bus = cp if self.CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG else cp_cam
|
|
speed_limit = bus.vl["FR_CMR_02_100ms"]["ISLW_SpdCluMainDis"]
|
|
else:
|
|
nav, cam = 0, 0
|
|
if self.CP_SP.flags & HyundaiFlagsSP.SPEED_LIMIT_AVAILABLE:
|
|
nav = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
|
|
if self.CP_SP.flags & HyundaiFlagsSP.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_sp: structs.CarStateSP, 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_SP.flags & HyundaiFlagsSP.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_SP.flags & HyundaiFlagsSP.NON_SCC_NO_FCA:
|
|
cp_cruise = cp if self.CP_SP.flags & HyundaiFlagsSP.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_sp.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_conv
|
|
|
|
def update_canfd_ext(self, ret: structs.CarState, ret_sp: structs.CarStateSP, 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_sp.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_factor
|