brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Actions 4 Packages Projects Releases Wiki Activity

clearpilot: initial commit of full source

Browse Source
This commit is contained in:
Brian Hanson
2026-04-11 06:25:25 +00:00
commit e2a0c1894a
3383 changed files with 834683 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

553
selfdrive/car/hyundai/carstate.py Executable file
View File

@@ -0,0 +1,553 @@
from collections import deque
import copy
import math
import sys
import json
from cereal import car
from openpilot.common.conversions import Conversions as CV
from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \
CANFD_CAR, Buttons, CarControllerParams
from openpilot.selfdrive.car.interfaces import CarStateBase
PREV_BUTTON_SAMPLES = 8
CLUSTER_SAMPLE_RATE = 20 # frames
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
"GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_SHIFTER"
if CP.carFingerprint in CANFD_CAR:
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.accelerator_msg_canfd = "ACCELERATOR" if CP.flags & HyundaiFlags.EV else \
"ACCELERATOR_ALT" if CP.flags & HyundaiFlags.HYBRID else \
"ACCELERATOR_BRAKE_ALT"
self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \
"CRUISE_BUTTONS"
self.is_metric = False
self.buttons_counter = 0
self.cruise_info = {}
# On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
self.cluster_speed = 0
self.cluster_speed_counter = CLUSTER_SAMPLE_RATE
self.params = CarControllerParams(CP)
# FrogPilot variables
self.main_enabled = False
# Clearpilot variables dev
self.fix_main_enabled_check = True
self.fix_main_enabled_executed = False
self.fix_main_enabled_cancel_main = False
self.lkas_was_pressed = False
self.lkas_trigger_result = None
def calculate_speed_limit(self, cp, cp_cam):
if self.CP.carFingerprint in CANFD_CAR:
speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
# print(speed_limit_bus.vl, sys.stderr)
# sys.stderr.write(str({k: v for k, v in vars(speed_limit_bus).items() if isinstance(v, (int, float, str, bool))}) + '\n')
# Clearpilot fix
best_speed = 0
# print(speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"])
# if (speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_3"]):
# best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_3"]
if (speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]):
if (best_speed == 0 or best_speed < speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]):
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]
if (best_speed == 0):
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
return best_speed
else:
if "SpeedLim_Nav_Clu" not in cp.vl["Navi_HU"]:
return 0
speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
return speed_limit if speed_limit not in (0, 255) else 0
def update(self, cp, cp_cam, frogpilot_variables):
if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam, frogpilot_variables)
ret = car.CarState.new_message()
cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
self.cluster_speed_counter += 1
if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
self.cluster_speed_counter = 0
# Mimic how dash converts to imperial.
# Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric
# TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this
if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,):
self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
ret.vEgoCluster = self.cluster_speed * speed_conv
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
# cruise state
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.available = self.main_enabled
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
ret.cruiseState.nonAdaptive = False
else:
ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 2. # Shows 'Cruise Control' on dash
ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2 # 2 includes regen braking by user on HEV/EV
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
if self.CP.flags & HyundaiFlags.HYBRID:
ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
else:
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
gear = cp.vl["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
if not self.CP.openpilotLongitudinalControl:
aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12"
aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
scc_warning = cp_cruise.vl["SCC12"]["TakeOverReq"] == 1 # sometimes only SCC system shows an FCW
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11 and CLU11
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
self.prev_main_buttons = self.main_buttons[-1]
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
self.prev_distance_button = self.distance_button
self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST
if self.CP.flags & HyundaiFlags.CAN_LFA_BTN:
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
self.params_memory.put_float("CarCruiseDisplayActual", cp_cruise.vl["SCC11"]["VSetDis"])
return ret
def update_canfd(self, cp, cp_cam, frogpilot_variables):
# ---- Preexisting unsorted frogpilot code ----
ret = car.CarState.new_message()
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
if self.CP.flags & (HyundaiFlags.EV | HyundaiFlags.HYBRID):
offset = 255. if self.CP.flags & HyundaiFlags.EV else 1023.
ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset
ret.gasPressed = ret.gas > 1e-5
else:
ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"])
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR"] == 1
ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT"] == 0
gear = cp.vl[self.gear_msg_canfd]["GEAR"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
# TODO: figure out positions
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
# TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP']
left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP"
if self.CP.carFingerprint == CAR.KONA_EV_2ND_GEN:
left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT"
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig],
cp.vl["BLINKERS"][right_blinker_sig])
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0
ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0
# cruise state
# CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
ret.cruiseState.available = self.main_enabled
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
else:
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1
ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
# Manual Speed Limit Assist is a feature that replaces non-adaptive cruise control on EV CAN FD platforms.
# It limits the vehicle speed, overridable by pressing the accelerator past a certain point.
# The car will brake, but does not respect positive acceleration commands in this mode
# TODO: find this message on ICE & HYBRID cars + cruise control signals (if exists)
if self.CP.flags & HyundaiFlags.EV:
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
self.cruise_can_msg = copy.copy(cp.vl_all[self.cruise_btns_msg_canfd])
# print(self.cruise_can_msg)
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
self.prev_main_buttons = self.main_buttons[-1]
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
# ------ End old frogpilot code ------
# Clearpilot Activation button fixes:
# This code tracks the engaged state of openpilot based on when the user clicks the
# cruise button. However, cruise only engages when the break is not pressed,
# and cruise can already be enabled when openpilot starts. This compensates for that behavior.
# Block attempt to engage if already engaged according to cruise state
# if ret.cruiseState.speed > 0 and self.main_enabled == False:
# self.main_buttons[-1] = 0
# Block attempt to engage if breaks are pressed
if self.main_buttons[-1] != 0 and ret.brakePressed and self.main_enabled == False:
self.main_buttons[-1] = 0
# Swicth to enabled based on button if above conditions pass
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
# --------- Resume preexisting frogpilot code ------------
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.flags & HyundaiFlags.CANFD_HDA2:
self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
else cp_cam.vl["CAM_0x2a4"])
self.prev_distance_button = self.distance_button
self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0
self.lkas_previously_enabled = self.lkas_enabled
lkas_enabled = False
try:
lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]
except:
nothing = 0
self.lkas_enabled = lkas_enabled
# if self.lkas_enabled and not self.lkas_previously_enabled:
# self.fix_main_enabled_cancel_main = True
# self.lkas_was_pressed = True
# print('Hello World', file=sys.stderr)
# sys.stderr.write(str({k: v for k, v in vars(ret).items() if isinstance(v, (int, float, str, bool))}) + '\n')
# print(ret, sys.stderr)
# print(ret.cruiseState, sys.stderr)
# print({"fix_main_enabled_check": self.fix_main_enabled_check}, sys.stderr)
# print({"fix_main_enabled_cancel_main": self.fix_main_enabled_cancel_main}, sys.stderr)
# print({"fix_main_enabled_executed": self.fix_main_enabled_executed}, sys.stderr)
# print({"self.lkas_enabled": self.lkas_enabled}, sys.stderr)
# print({"lkas_enabled": lkas_enabled}, sys.stderr)
# print({"lkas_was_pressed": self.lkas_was_pressed}, sys.stderr)
# print({"lkas_trigger_result": self.lkas_trigger_result}, sys.stderr)
# notes on self:
# lkas_enabled = 1 = lkas button has been pressed
# prev_cruise_buttons = 0 (none), 1, 2 - up down
# new lane change works perfect
# need to auto disable lat on turn lower than 20 + turn signal as default
# ret: cruiseState.speed > 0 = and cruiseState.enabled = false = idle cruise.
# press cruise to cancel it if not op engaged
# engaged at 25
# ret from carstate:
# . cruiseState = (
# enabled = true,
# speed = 11.176, # above 0 if disengaged but set
# available = true,
# speedOffset = 0,
# standstill = false,
# nonAdaptive = false,
# speedCluster = 0 ),
# .standstill = false,
# steeringPressed = false,
# Interesting values:
# 416.ACC_ObjDist - distance to lead radar?
# 506.SPEED_LIMIT_2
# print(speed_limit_bus.vl, sys.stderr)
# {416: {'CHECKSUM': 34571.0, 'NEW_SIGNAL_1': 0.0, 'NEW_SIGNAL_8': 0.0, 'ZEROS': 0.0, 'ZEROS_3':
# 0.0, 'ZEROS_4': 0.0, 'ZEROS_6': 256.0, 'ZEROS_8': 0.0, 'NEW_SIGNAL_3': 0.0,
# 'SET_ME_TMP_64': 100.0, 'SET_ME_2': 4.0, 'NEW_SIGNAL_6': 0.0, 'COUNTER': 173.0,
# 'ZEROS_9': 0.0, 'ZEROS_10': 0.0, 'SET_ME_3': 3.0, 'ObjValid': 0.0, 'NEW_SIGNAL_2': 0.0,
# 'OBJ_STATUS': 0.0, 'ACC_ObjDist': 204.60000000000002, 'ZEROS_5': 0.0, 'DISTANCE_SETTING': 0.0,
# 'ZEROS_2': 0.0, 'CRUISE_STANDSTILL': 0.0, 'aReqRaw': 0.0, 'aReqValue': 0.0, 'ZEROS_7': 0.0,
# 'ACCMode': 0.0, 'NEW_SIGNAL_12': 16.400000000000002, 'JerkLowerLimit': 0.0, 'StopReq': 0.0,
# 'NEW_SIGNAL_15': 204.60000000000002, 'VSetDis': 0.0, 'MainMode_ACC': 0.0,
# 'JerkUpperLimit': 0.0}, 'SCC_CONTROL': {'CHECKSUM': 34571.0, 'NEW_SIGNAL_1': 0.0,
# 'NEW_SIGNAL_8': 0.0, 'ZEROS': 0.0, 'ZEROS_3': 0.0, 'ZEROS_4': 0.0, 'ZEROS_6': 256.0,
# 'ZEROS_8': 0.0, 'NEW_SIGNAL_3': 0.0, 'SET_ME_TMP_64': 100.0, 'SET_ME_2': 4.0, 'NEW_SIGNAL_6': 0.0,
# 'COUNTER': 173.0, 'ZEROS_9': 0.0, 'ZEROS_10': 0.0, 'SET_ME_3': 3.0, 'ObjValid': 0.0,
# 'NEW_SIGNAL_2': 0.0, 'OBJ_STATUS': 0.0, 'ACC_ObjDist': 204.60000000000002, 'ZEROS_5': 0.0,
# 'DISTANCE_SETTING': 0.0, 'ZEROS_2': 0.0, 'CRUISE_STANDSTILL': 0.0, 'aReqRaw': 0.0,
# 'aReqValue': 0.0, 'ZEROS_7': 0.0, 'ACCMode': 0.0, 'NEW_SIGNAL_12': 16.400000000000002,
# 'JerkLowerLimit': 0.0, 'StopReq': 0.0, 'NEW_SIGNAL_15': 204.60000000000002,
# 'VSetDis': 0.0, 'MainMode_ACC': 0.0, 'JerkUpperLimit': 0.0},
# 506: {'SPEED_LIMIT_3': 38.0,
# 'SPEED_LIMIT_2': 35.0, 'SPEED_LIMIT_1': 35.0, 'SPEED_CHANGE_BLINKING': 0.0, 'CHIME_2': 0.0,
# 'CHIME_1': 0.0, 'ARROW_DOWN': 0.0, 'ARROW_UP': 0.0, 'SECONDARY_LIMIT_1': 0.0,
# 'SECONDARY_LIMIT_2': 0.0, 'SCHOOL_ZONE': 0.0},
# 'CLUSTER_SPEED_LIMIT': {'SPEED_LIMIT_3': 38.0,
# 'SPEED_LIMIT_2': 35.0, 'SPEED_LIMIT_1': 35.0, 'SPEED_CHANGE_BLINKING': 0.0, 'CHIME_2': 0.0,
# 'CHIME_1': 0.0, 'ARROW_DOWN': 0.0, 'ARROW_UP': 0.0, 'SECONDARY_LIMIT_1': 0.0,
# 'SECONDARY_LIMIT_2': 0.0, 'SCHOOL_ZONE': 0.0}}
# <_io.TextIOWrapper name='<stderr>' mode='w' encoding='utf-8'>
# ( enabled = false,
# speed = 9.83488,
# available = false,
# speedOffset = 0,
# standstill = false,
# nonAdaptive = false,
# speedCluster = 0 )
# print("Set limit")
# print(self.calculate_speed_limit(cp, cp_cam))
# self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
return ret
def get_can_parser(self, CP):
if CP.carFingerprint in CANFD_CAR:
return self.get_can_parser_canfd(CP)
messages = [
# address, frequency
("MDPS12", 50),
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("CLU15", 5),
("ESP12", 100),
("CGW1", 10),
("CGW2", 5),
("CGW4", 5),
("WHL_SPD11", 50),
("SAS11", 100),
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR:
messages += [
("SCC11", 50),
("SCC12", 50),
]
if CP.flags & HyundaiFlags.USE_FCA.value:
messages.append(("FCA11", 50))
if CP.enableBsm:
messages.append(("LCA11", 50))
if CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
messages.append(("E_EMS11", 50))
else:
messages += [
("EMS12", 100),
("EMS16", 100),
]
if CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
messages.append(("ELECT_GEAR", 20))
elif CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
pass
elif CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
messages.append(("TCU12", 100))
else:
messages.append(("LVR12", 100))
if CP.flags & HyundaiFlags.CAN_LFA_BTN:
messages.append(("BCM_PO_11", 50))
messages += [
("Navi_HU", 5),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
def get_cam_can_parser(CP):
if CP.carFingerprint in CANFD_CAR:
return CarState.get_cam_can_parser_canfd(CP)
messages = [
("LKAS11", 100)
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR:
messages += [
("SCC11", 50),
("SCC12", 50),
]
if CP.flags & HyundaiFlags.USE_FCA.value:
messages.append(("FCA11", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
def get_can_parser_canfd(self, CP):
messages = [
(self.gear_msg_canfd, 100),
(self.accelerator_msg_canfd, 100),
("WHEEL_SPEEDS", 100),
("STEERING_SENSORS", 100),
("MDPS", 100),
("TCS", 50),
("CRUISE_BUTTONS_ALT", 50),
("BLINKERS", 4),
("DOORS_SEATBELTS", 4),
]
if CP.flags & HyundaiFlags.EV:
messages += [
("MANUAL_SPEED_LIMIT_ASSIST", 10),
]
if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
messages += [
("CRUISE_BUTTONS", 50)
]
if CP.enableBsm:
messages += [
("BLINDSPOTS_REAR_CORNERS", 20),
]
if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl:
messages += [
("SCC_CONTROL", 50),
]
if CP.flags & HyundaiFlags.CANFD_HDA2:
messages.append(("CLUSTER_SPEED_LIMIT", 10))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
@staticmethod
def get_cam_can_parser_canfd(CP):
messages = []
if CP.flags & HyundaiFlags.CANFD_HDA2:
block_lfa_msg = "CAM_0x362" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "CAM_0x2a4"
messages += [(block_lfa_msg, 20)]
elif CP.flags & HyundaiFlags.CANFD_CAMERA_SCC:
messages += [
("SCC_CONTROL", 50),
]
if not (CP.flags & HyundaiFlags.CANFD_HDA2):
messages.append(("CLUSTER_SPEED_LIMIT", 10))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)