brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-03-09 10:33:28 -06:00
parent cd3bd61918
commit f6831761bf
16 changed files with 5259 additions and 643 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
selfdrive/car/hyundai org/__init__.py Normal file
View File

210
selfdrive/car/hyundai org/carcontroller.org Normal file
View File

@@ -0,0 +1,210 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
# All slightly below EPS thresholds to avoid fault
MAX_ANGLE = 85
MAX_ANGLE_FRAMES = 89
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
# initialize to no line visible
# TODO: this is not accurate for all cars
sys_state = 1
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
sys_state = 3 if enabled or sys_warning else 4
elif hud_control.leftLaneVisible:
sys_state = 5
elif hud_control.rightLaneVisible:
sys_state = 6
# initialize to no warnings
left_lane_warning = 0
right_lane_warning = 0
if hud_control.leftLaneDepart:
left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
if hud_control.rightLaneDepart:
right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
return sys_warning, sys_state, left_lane_warning, right_lane_warning
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.CAN = CanBus(CP)
self.params = CarControllerParams(CP)
self.packer = CANPacker(dbc_name)
self.angle_limit_counter = 0
self.frame = 0
self.accel_last = 0
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.last_button_frame = 0
def update(self, CC, CS, now_nanos, sport_plus):
actuators = CC.actuators
hud_control = CC.hudControl
# steering torque
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
# >90 degree steering fault prevention
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
self.angle_limit_counter, MAX_ANGLE_FRAMES,
MAX_ANGLE_CONSECUTIVE_FRAMES)
if not CC.latActive:
apply_steer = 0
# Hold torque with induced temporary fault when cutting the actuation bit
torque_fault = CC.latActive and not apply_steer_req
self.apply_steer_last = apply_steer
# accel + longitudinal
if sport_plus:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS)
else:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
can_sends = []
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, self.CAN.ECAN
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
# CAN-FD platforms
if self.CP.carFingerprint in CANFD_CAR:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING))
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
if self.CP.openpilotLongitudinalControl:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, CS.personality_profile))
self.accel_last = accel
else:
# button presses
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, CS.out.cruiseState.available, CS.personality_profile))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(hyundaican.create_acc_opt(self.packer))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool):
can_sends = []
if use_clu11:
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint))
elif CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
else:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# TODO: resume for alt button cars
pass
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
return can_sends

500
selfdrive/car/hyundai org/carstate.org Normal file
View File

@@ -0,0 +1,500 @@
from collections import deque
import copy
import math
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, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
from openpilot.common.watcher import Watcher
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.carFingerprint in EV_CAR else \
"ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR 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)
def update(self, cp, cp_cam):
Watcher.log_watch("hyundai_carstate_update_cp", cp)
Watcher.log_watch("hyundai_carstate_update_cp_cam", cp_cam)
if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam)
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.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
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.carFingerprint in (HYBRID_CAR | EV_CAR):
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
# BBot functions for lfa and gap buttons - test speed up / down
self.oscar_lane_center_btn_pressed= False
self.oscar_slc_decel = False
self.oscar_slc_accel = False
self.param_memory.put("oscar_debug", "Hello World")
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# self.custom_speed_up = True
# self.lkas_previously_pressed = lkas_pressed
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
Watcher.log_watch("hyundai_carstate_update_self", self)
return ret
def update_canfd(self, cp, cp_cam):
Watcher.log_watch("hyundai_carstate_update_canfd_cp", cp)
Watcher.log_watch("hyundai_carstate_update_canfd_cp_cam", cp_cam)
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.carFingerprint in (EV_CAR | HYBRID_CAR):
offset = 255. if self.CP.carFingerprint in EV_CAR 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.carFingerprint in EV_CAR:
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
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"])
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
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"])
SpeedLimitController.load_state()
SpeedLimitController.car_speed_limit = self.calculate_speed_limit_canfd(self.CP, cp, cp_cam) * speed_factor
SpeedLimitController.write_car_state()
# self.custom_speed_up = False
self.oscar_lane_center_btn_pressed = False
if ret.cruiseState.available:
if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
self.oscar_lane_center_btn_pressed = True
lkas_pressed = False
try:
lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
except:
nothing = 0
# intentionally cause a failure
if lkas_pressed:
floog=norpdywoop
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
Watcher.log_watch("hyundai_carstate_update_canfd_self", self)
return ret
# BBOT does not work
def calculate_speed_limit_canfd(self, CP, cp, cp_cam):
try:
self._speed_limit_clu = cp.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
return self._speed_limit_clu if self._speed_limit_clu not in (0, 255) else 0
except:
return 0
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.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("E_EMS11", 50))
else:
messages += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
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))
messages.append(("BCM_PO_11", 50))
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.carFingerprint in EV_CAR:
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 CP.nav_msg:
# messages.append(("CLUSTER_SPEED_LIMIT", 10))
if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl:
messages += [
("SCC_CONTROL", 50),
]
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),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)

1710
selfdrive/car/hyundai org/fingerprints.org Normal file
View File

File diff suppressed because it is too large Load Diff

213
selfdrive/car/hyundai org/hyundaican.org Normal file
View File

@@ -0,0 +1,213 @@
import crcmod
from openpilot.selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR
hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
def create_lkas11(packer, frame, car_fingerprint, 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 car_fingerprint 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 car_fingerprint 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 car_fingerprint == 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 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, car_fingerprint):
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 car_fingerprint in CAMERA_SCC_CAR else 0
return packer.make_can_msg("CLU11", bus, values)
def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
values = {
"LFA_Icon_State": 2 if enabled 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, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available, personality):
commands = []
scc11_values = {
"MainMode_ACC": 1 if cruise_available else 0,
"TauGapSet": personality + 1,
"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 lead_visible 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)

224
selfdrive/car/hyundai org/hyundaicanfd.org Normal file
View File

@@ -0,0 +1,224 @@
from openpilot.common.numpy_fast import clip
from openpilot.selfdrive.car import CanBusBase
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
class CanBus(CanBusBase):
def __init__(self, CP, hda2=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
if hda2 is None:
assert CP is not None
hda2 = CP.flags & HyundaiFlags.CANFD_HDA2.value
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars
# have a different harness than the HDA1 and non-HDA variants in order to split
# a different bus, since the steering is done by different ECUs.
self._a, self._e = 1, 0
if hda2:
self._a, self._e = 0, 1
self._a += self.offset
self._e += self.offset
self._cam = 2 + self.offset
@property
def ECAN(self):
return self._e
@property
def ACAN(self):
return self._a
@property
def CAM(self):
return self._cam
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
ret = []
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1 if lat_active else 0,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
"STEER_MODE": 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
"NEW_SIGNAL_1": 0,
"NEW_SIGNAL_2": 0,
}
if CP.flags & HyundaiFlags.CANFD_HDA2:
hda2_lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "LKAS"
if CP.openpilotLongitudinalControl:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
ret.append(packer.make_can_msg(hda2_lkas_msg, CAN.ACAN, values))
else:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
return ret
def create_suppress_lfa(packer, CAN, hda2_lfa_block_msg, hda2_alt_steering):
suppress_msg = "CAM_0x362" if hda2_alt_steering else "CAM_0x2a4"
msg_bytes = 32 if hda2_alt_steering else 24
values = {f"BYTE{i}": hda2_lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7}
values["COUNTER"] = hda2_lfa_block_msg["COUNTER"]
values["SET_ME_0"] = 0
values["SET_ME_0_2"] = 0
values["LEFT_LANE_LINE"] = 0
values["RIGHT_LANE_LINE"] = 0
return packer.make_can_msg(suppress_msg, CAN.ACAN, values)
def create_buttons(packer, CP, CAN, cnt, btn):
values = {
"COUNTER": cnt,
"SET_ME_1": 1,
"CRUISE_BUTTONS": btn,
}
bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
# TODO: why do we copy different values here?
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"NEW_SIGNAL_1",
"MainMode_ACC",
"ACCMode",
"ZEROS_9",
"CRUISE_STANDSTILL",
"ZEROS_5",
"DISTANCE_SETTING",
"VSetDis",
]}
else:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"ACCMode",
"VSetDis",
"CRUISE_STANDSTILL",
]}
values.update({
"ACCMode": 4,
"aReqRaw": 0.0,
"aReqValue": 0.0,
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CAN, enabled):
values = {
"HDA_ICON": 1 if enabled else 0,
# 0 off, 1 gray, 2 green, 3 blinking (wheel icon)
"LFA_ICON": 0 if enabled else 0, # was green before i think in 2? 3 should be flashing?
# "LFA_ICON": 2 if enabled else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, personality):
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
a_val, a_raw = 0, 0
else:
a_raw = accel
a_val = clip(accel, accel_last - jn, accel_last + jn)
values = {
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
"MainMode_ACC": 1,
"StopReq": 1 if stopping else 0,
"aReqValue": a_val,
"aReqRaw": a_raw,
"VSetDis": set_speed,
"JerkLowerLimit": jerk if enabled else 1,
"JerkUpperLimit": 3.0,
"ACC_ObjDist": 1,
"ObjValid": 0,
"OBJ_STATUS": 2,
"SET_ME_2": 0x4,
"SET_ME_3": 0x3,
"SET_ME_TMP_64": 0x64,
"DISTANCE_SETTING": personality + 1,
}
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
ret = []
values = {
}
ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values))
blink = 0
if left_blink:
blink = 3
elif right_blink:
blink = 4
values = {
"BLINKER_CONTROL": blink,
}
ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values))
return ret
def create_adrv_messages(packer, CAN, frame):
# messages needed to car happy after disabling
# the ADAS Driving ECU to do longitudinal control
ret = []
values = {
}
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
if frame % 2 == 0:
values = {
'AEB_SETTING': 0x1, # show AEB disabled icon
'SET_ME_2': 0x2,
'SET_ME_FF': 0xff,
'SET_ME_FC': 0xfc,
'SET_ME_9': 0x9,
}
ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
if frame % 5 == 0:
values = {
'SET_ME_1C': 0x1c,
'SET_ME_FF': 0xff,
'SET_ME_TMP_F': 0xf,
'SET_ME_TMP_F_2': 0xf,
}
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))
values = {
'SET_ME_E1': 0xe1,
'SET_ME_3A': 0x3a,
}
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values))
if frame % 20 == 0:
values = {
'SET_ME_15': 0x15,
}
ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values))
if frame % 100 == 0:
values = {
'SET_ME_22': 0x22,
'SET_ME_41': 0x41,
}
ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values))
return ret

375
selfdrive/car/hyundai org/interface.org Normal file
View File

@@ -0,0 +1,375 @@
from cereal import car
from panda import Panda
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
CANFD_UNSUPPORTED_LONGITUDINAL_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \
UNSUPPORTED_LONGITUDINAL_CAR, Buttons
from openpilot.selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.disable_ecu import disable_ecu
from openpilot.common.params import Params
params_memory = Params("/dev/shm/params")
Ecu = car.CarParams.Ecu
SafetyModel = car.CarParams.SafetyModel
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL)
BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise,
Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel}
def set_safety_config_hyundai(candidate, CAN, can_fd=False):
platform = SafetyModel.hyundaiCanfd if can_fd else \
SafetyModel.hyundaiLegacy if candidate in LEGACY_SAFETY_MODE_CAR else \
SafetyModel.hyundai
cfgs = [get_safety_config(platform), ]
if CAN.ECAN >= 4:
cfgs.insert(0, get_safety_config(SafetyModel.noOutput))
return cfgs
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "hyundai"
ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None
params_memory.put_bool("CSLCAvailable", True)
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to selfdrive/car/tests/routes.py, we can remove it from this list.
# FIXME: the Optima Hybrid 2017 uses a different SCC12 checksum
ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, }
hda2 = Ecu.adas in [fw.ecu for fw in car_fw]
CAN = CanBus(None, hda2, fingerprint)
if candidate in CANFD_CAR:
# detect HDA2 with ADAS Driving ECU
if hda2:
if 0x110 in fingerprint[CAN.CAM]:
ret.flags |= HyundaiFlags.CANFD_HDA2_ALT_STEERING.value
params_memory.put_bool("CSLCAvailable", False)
else:
# non-HDA2
if 0x1cf not in fingerprint[CAN.ECAN]:
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
# ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead
if 0x130 not in fingerprint[CAN.ECAN]:
if 0x40 not in fingerprint[CAN.ECAN]:
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS_2.value
else:
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value
if candidate not in CANFD_RADAR_SCC_CAR:
ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value
else:
# Send LFA message on cars with HDA
if 0x485 in fingerprint[2]:
ret.flags |= HyundaiFlags.SEND_LFA.value
# These cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
if 0x38d in fingerprint[0] or 0x38d in fingerprint[2]:
ret.flags |= HyundaiFlags.USE_FCA.value
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerLimitTimer = 0.4
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate in (CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN):
ret.mass = 1600. if candidate == CAR.AZERA_6TH_GEN else 1675. # ICE is ~average of 2.5L and 3.5L
ret.wheelbase = 2.885
ret.steerRatio = 14.5
elif candidate in (CAR.SANTA_FE, CAR.SANTA_FE_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
ret.mass = 3982. * CV.LB_TO_KG
ret.wheelbase = 2.766
# Values from optimizer
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
ret.tireStiffnessFactor = 0.82
elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID):
ret.mass = 1513.
ret.wheelbase = 2.84
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.SONATA_LF:
ret.mass = 1536.
ret.wheelbase = 2.804
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
elif candidate == CAR.PALISADE:
ret.mass = 1999.
ret.wheelbase = 2.90
ret.steerRatio = 15.6 * 1.15
ret.tireStiffnessFactor = 0.63
elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30):
ret.mass = 1275.
ret.wheelbase = 2.7
ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
ret.tireStiffnessFactor = 0.385 # stiffnessFactor settled on 1.0081302973865127
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.ELANTRA_2021:
ret.mass = 2800. * CV.LB_TO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.ELANTRA_HEV_2021:
ret.mass = 3017. * CV.LB_TO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.HYUNDAI_GENESIS:
ret.mass = 2060.
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.minSteerSpeed = 60 * CV.KPH_TO_MS
elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, CAR.KONA_EV_2ND_GEN):
ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743., CAR.KONA_EV_2ND_GEN: 1740.}.get(candidate, 1275.)
ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6)
ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42) # Spec
ret.tireStiffnessFactor = 0.385
elif candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019, CAR.IONIQ_HEV_2022, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV):
ret.mass = 1490. # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
ret.steerRatio = 13.73 # Spec
ret.tireStiffnessFactor = 0.385
if candidate in (CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV_2019):
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate in (CAR.IONIQ_5, CAR.IONIQ_6):
ret.mass = 1948
ret.wheelbase = 2.97
ret.steerRatio = 14.26
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.VELOSTER:
ret.mass = 2917. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75 * 1.15
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.TUCSON:
ret.mass = 3520. * CV.LB_TO_KG
ret.wheelbase = 2.67
ret.steerRatio = 14.00 * 1.15
ret.tireStiffnessFactor = 0.385
elif candidate in (CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN):
ret.steerActuatorDelay = 0.1 # Or .15? BBOT test oscarpilot
ret.mass = 1630. # average
ret.wheelbase = 2.756
ret.steerRatio = 16.
ret.tireStiffnessFactor = 0.385
elif candidate == CAR.SANTA_CRUZ_1ST_GEN:
ret.mass = 1870. # weight from Limited trim - the only supported trim
ret.wheelbase = 3.000
# steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf
ret.steerRatio = 14.2
elif candidate == CAR.CUSTIN_1ST_GEN:
ret.mass = 1690. # from https://www.hyundai-motor.com.tw/clicktobuy/custin#spec_0
ret.wheelbase = 3.055
ret.steerRatio = 17.0 # from learner
elif candidate == CAR.STARIA_4TH_GEN:
ret.mass = 2205.
ret.wheelbase = 3.273
ret.steerRatio = 11.94 # https://www.hyundai.com/content/dam/hyundai/au/en/models/staria-load/premium-pip-update-2023/spec-sheet/STARIA_Load_Spec-Table_March_2023_v3.1.pdf
# Kia
elif candidate == CAR.KIA_SORENTO:
ret.mass = 1985.
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
elif candidate in (CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_PHEV_2022):
ret.mass = 3543. * CV.LB_TO_KG # average of all the cars
ret.wheelbase = 2.7
ret.steerRatio = 13.6 # average of all the cars
ret.tireStiffnessFactor = 0.385
if candidate == CAR.KIA_NIRO_PHEV:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.KIA_SELTOS:
ret.mass = 1337.
ret.wheelbase = 2.63
ret.steerRatio = 14.56
elif candidate == CAR.KIA_SPORTAGE_5TH_GEN:
ret.mass = 1700. # weight from SX and above trims, average of FWD and AWD versions
ret.wheelbase = 2.756
ret.steerRatio = 13.6 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications
elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL):
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
ret.tireStiffnessFactor = 0.5
if candidate == CAR.KIA_OPTIMA_G4:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate in (CAR.KIA_STINGER, CAR.KIA_STINGER_2022):
ret.mass = 1825.
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
elif candidate == CAR.KIA_FORTE:
ret.mass = 2878. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.KIA_CEED:
ret.mass = 1450.
ret.wheelbase = 2.65
ret.steerRatio = 13.75
ret.tireStiffnessFactor = 0.5
elif candidate in (CAR.KIA_K5_2021, CAR.KIA_K5_HEV_2020):
ret.mass = 3381. * CV.LB_TO_KG
ret.wheelbase = 2.85
ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims)
ret.tireStiffnessFactor = 0.5
elif candidate == CAR.KIA_EV6:
ret.mass = 2055
ret.wheelbase = 2.9
ret.steerRatio = 16.
ret.tireStiffnessFactor = 0.65
elif candidate == CAR.KIA_SPORTAGE_HYBRID_5TH_GEN:
ret.mass = 1767. # SX Prestige trim support only
ret.wheelbase = 2.756
ret.steerRatio = 13.6
elif candidate in (CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN):
ret.wheelbase = 2.81
ret.steerRatio = 13.5 # average of the platforms
if candidate == CAR.KIA_SORENTO_4TH_GEN:
ret.mass = 3957 * CV.LB_TO_KG
elif candidate == CAR.KIA_SORENTO_HEV_4TH_GEN:
ret.mass = 4255 * CV.LB_TO_KG
else:
ret.mass = 4537 * CV.LB_TO_KG
elif candidate == CAR.KIA_CARNIVAL_4TH_GEN:
ret.mass = 2087.
ret.wheelbase = 3.09
ret.steerRatio = 14.23
elif candidate == CAR.KIA_K8_HEV_1ST_GEN:
ret.mass = 1630. # https://carprices.ae/brands/kia/2023/k8/1.6-turbo-hybrid
ret.wheelbase = 2.895
ret.steerRatio = 13.27 # guesstimate from K5 platform
# Genesis
elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN:
ret.mass = 2205
ret.wheelbase = 2.9
# https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1.
ret.steerRatio = 12.6
elif candidate == CAR.GENESIS_G70:
ret.steerActuatorDelay = 0.1
ret.mass = 1640.0
ret.wheelbase = 2.84
ret.steerRatio = 13.56
elif candidate == CAR.GENESIS_G70_2020:
ret.mass = 3673.0 * CV.LB_TO_KG
ret.wheelbase = 2.83
ret.steerRatio = 12.9
elif candidate == CAR.GENESIS_GV70_1ST_GEN:
ret.mass = 1950.
ret.wheelbase = 2.87
ret.steerRatio = 14.6
elif candidate == CAR.GENESIS_G80:
ret.mass = 2060.
ret.wheelbase = 3.01
ret.steerRatio = 16.5
elif candidate == CAR.GENESIS_G90:
ret.mass = 2200.
ret.wheelbase = 3.15
ret.steerRatio = 12.069
elif candidate == CAR.GENESIS_GV80:
ret.mass = 2258.
ret.wheelbase = 2.95
ret.steerRatio = 14.14
# *** longitudinal control ***
if candidate in CANFD_CAR:
ret.longitudinalTuning.kpV = [0.1]
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR)
else:
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.stoppingControl = True
ret.startingState = True
ret.vEgoStarting = 0.1
ret.startAccel = 1.0
ret.longitudinalActuatorDelayLowerBound = 0.5
ret.longitudinalActuatorDelayUpperBound = 0.5
# *** feature detection ***
# if candidate in CANFD_CAR:
# ret.enableBsm = 0x1e5 in fingerprint[CAN.ECAN]
# ret.nav_msg = 0x544 in fingerprint[0]
# else:
# ret.enableBsm = 0x58b in fingerprint[0]
# ret.nav_msg = False
# *** panda safety config ***
ret.safetyConfigs = set_safety_config_hyundai(candidate, CAN, can_fd=(candidate in CANFD_CAR))
if hda2:
ret.flags |= HyundaiFlags.CANFD_HDA2.value
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2
if candidate in CANFD_CAR:
if hda2 and ret.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING
if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC or candidate in CAMERA_SCC_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
params_memory.put_bool("CSLCAvailable", False)
if candidate in HYBRID_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_HYBRID_GAS
elif candidate in EV_CAR:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS
if candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022):
ret.flags |= HyundaiFlags.ALT_LIMITS.value
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS
ret.centerToFront = ret.wheelbase * 0.4
return ret
@staticmethod
def init(CP, logcan, sendcan):
if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value):
addr, bus = 0x7d0, 0
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, CanBus(CP).ECAN
disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
# for blinkers
if CP.flags & HyundaiFlags.ENABLE_BLINKERS:
disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01')
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
if self.CS.CP.openpilotLongitudinalControl:
ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)
# On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
# To avoid re-engaging when openpilot cancels, check user engagement intention via buttons
# Main button also can trigger an engagement on these cars
allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons)
events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable)
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
if self.low_speed_alert:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.events = events.to_msg()
return ret
def apply(self, c, now_nanos, sport_plus):
return self.CC.update(c, self.CS, now_nanos, sport_plus)

79
selfdrive/car/hyundai org/radar_interface.org Normal file
View File

@@ -0,0 +1,79 @@
import math
from cereal import car
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
from openpilot.selfdrive.car.hyundai.values import DBC
RADAR_START_ADDR = 0x500
RADAR_MSG_COUNT = 32
# POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/
def get_radar_can_parser(CP):
if DBC[CP.carFingerprint]['radar'] is None:
return None
messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT)]
return CANParser(DBC[CP.carFingerprint]['radar'], messages, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.updated_messages = set()
self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
self.track_id = 0
self.radar_off_can = CP.radarUnavailable
self.rcp = get_radar_can_parser(CP)
def update(self, can_strings):
if self.radar_off_can or (self.rcp is None):
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
def _update(self, updated_messages):
ret = car.RadarData.new_message()
if self.rcp is None:
return ret
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"]
if addr not in self.pts:
self.pts[addr] = car.RadarData.RadarPoint.new_message()
self.pts[addr].trackId = self.track_id
self.track_id += 1
valid = msg['STATE'] in (3, 4)
if valid:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[addr].measured = True
self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
self.pts[addr].vRel = msg['REL_SPEED']
self.pts[addr].aRel = msg['REL_ACCEL']
self.pts[addr].yvRel = float('nan')
else:
del self.pts[addr]
ret.points = list(self.pts.values())
return ret

591
selfdrive/car/hyundai org/values.org Normal file
View File

@@ -0,0 +1,591 @@
import re
from dataclasses import dataclass
from enum import Enum, IntFlag, StrEnum
from typing import Dict, List, Optional, Set, Tuple, Union
from cereal import car
from panda.python import uds
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = car.CarParams.Ecu
class CarControllerParams:
ACCEL_MIN = -3.5 # m/s
ACCEL_MAX = 2.0 # m/s
ACCEL_MAX_PLUS = 4.0 # m/s
def __init__(self, CP):
self.STEER_DELTA_UP = 3
self.STEER_DELTA_DOWN = 7
self.STEER_DRIVER_ALLOWANCE = 50
self.STEER_DRIVER_MULTIPLIER = 2
self.STEER_DRIVER_FACTOR = 1
self.STEER_THRESHOLD = 150
self.STEER_STEP = 1 # 100 Hz
if CP.carFingerprint in CANFD_CAR:
self.STEER_MAX = 270
self.STEER_DRIVER_ALLOWANCE = 250
self.STEER_DRIVER_MULTIPLIER = 2
self.STEER_THRESHOLD = 250
self.STEER_DELTA_UP = 2
self.STEER_DELTA_DOWN = 3
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
# If the max stock LKAS request is <384, add your car to this list.
elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.IONIQ,
CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV,
CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_SORENTO):
self.STEER_MAX = 255
# these cars have significantly more torque than most HKG; limit to 70% of max
elif CP.flags & HyundaiFlags.ALT_LIMITS:
self.STEER_MAX = 270
self.STEER_DELTA_UP = 2
self.STEER_DELTA_DOWN = 3
# Default for most HKG
else:
self.STEER_MAX = 384
class HyundaiFlags(IntFlag):
CANFD_HDA2 = 1
CANFD_ALT_BUTTONS = 2
CANFD_ALT_GEARS = 4
CANFD_CAMERA_SCC = 8
ALT_LIMITS = 16
ENABLE_BLINKERS = 32
CANFD_ALT_GEARS_2 = 64
SEND_LFA = 128
USE_FCA = 256
CANFD_HDA2_ALT_STEERING = 512
class CAR(StrEnum):
# Hyundai
AZERA_6TH_GEN = "HYUNDAI AZERA 6TH GEN"
AZERA_HEV_6TH_GEN = "HYUNDAI AZERA HYBRID 6TH GEN"
ELANTRA = "HYUNDAI ELANTRA 2017"
ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
ELANTRA_2021 = "HYUNDAI ELANTRA 2021"
ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021"
HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016"
IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019"
IONIQ_HEV_2022 = "HYUNDAI IONIQ HYBRID 2020-2022"
IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019"
IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020"
IONIQ_PHEV_2019 = "HYUNDAI IONIQ PLUG-IN HYBRID 2019"
IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020"
KONA = "HYUNDAI KONA 2020"
KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
KONA_EV_2022 = "HYUNDAI KONA ELECTRIC 2022"
KONA_EV_2ND_GEN = "HYUNDAI KONA ELECTRIC 2ND GEN"
KONA_HEV = "HYUNDAI KONA HYBRID 2020"
SANTA_FE = "HYUNDAI SANTA FE 2019"
SANTA_FE_2022 = "HYUNDAI SANTA FE 2022"
SANTA_FE_HEV_2022 = "HYUNDAI SANTA FE HYBRID 2022"
SANTA_FE_PHEV_2022 = "HYUNDAI SANTA FE PlUG-IN HYBRID 2022"
SONATA = "HYUNDAI SONATA 2020"
SONATA_LF = "HYUNDAI SONATA 2019"
STARIA_4TH_GEN = "HYUNDAI STARIA 4TH GEN"
TUCSON = "HYUNDAI TUCSON 2019"
PALISADE = "HYUNDAI PALISADE 2020"
VELOSTER = "HYUNDAI VELOSTER 2019"
SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021"
IONIQ_5 = "HYUNDAI IONIQ 5 2022"
IONIQ_6 = "HYUNDAI IONIQ 6 2023"
TUCSON_4TH_GEN = "HYUNDAI TUCSON 4TH GEN"
TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN"
SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN"
CUSTIN_1ST_GEN = "HYUNDAI CUSTIN 1ST GEN"
# Kia
KIA_FORTE = "KIA FORTE E 2018 & GT 2021"
KIA_K5_2021 = "KIA K5 2021"
KIA_K5_HEV_2020 = "KIA K5 HYBRID 2020"
KIA_K8_HEV_1ST_GEN = "KIA K8 HYBRID 1ST GEN"
KIA_NIRO_EV = "KIA NIRO EV 2020"
KIA_NIRO_EV_2ND_GEN = "KIA NIRO EV 2ND GEN"
KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019"
KIA_NIRO_PHEV_2022 = "KIA NIRO PLUG-IN HYBRID 2022"
KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021"
KIA_NIRO_HEV_2ND_GEN = "KIA NIRO HYBRID 2ND GEN"
KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN"
KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT"
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
KIA_OPTIMA_H_G4_FL = "KIA OPTIMA HYBRID 4TH GEN FACELIFT"
KIA_SELTOS = "KIA SELTOS 2021"
KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN"
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
KIA_SORENTO_4TH_GEN = "KIA SORENTO 4TH GEN"
KIA_SORENTO_HEV_4TH_GEN = "KIA SORENTO HYBRID 4TH GEN"
KIA_SORENTO_PHEV_4TH_GEN = "KIA SORENTO PLUG-IN HYBRID 4TH GEN"
KIA_SPORTAGE_HYBRID_5TH_GEN = "KIA SPORTAGE HYBRID 5TH GEN"
KIA_STINGER = "KIA STINGER GT2 2018"
KIA_STINGER_2022 = "KIA STINGER 2022"
KIA_CEED = "KIA CEED INTRO ED 2019"
KIA_EV6 = "KIA EV6 2022"
KIA_CARNIVAL_4TH_GEN = "KIA CARNIVAL 4TH GEN"
# Genesis
GENESIS_GV60_EV_1ST_GEN = "GENESIS GV60 ELECTRIC 1ST GEN"
GENESIS_G70 = "GENESIS G70 2018"
GENESIS_G70_2020 = "GENESIS G70 2020"
GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN"
GENESIS_G80 = "GENESIS G80 2017"
GENESIS_G90 = "GENESIS G90 2017"
GENESIS_GV80 = "GENESIS GV80 2023"
class Footnote(Enum):
CANFD = CarFootnote(
"Requires a <a href=\"https://comma.ai/shop/can-fd-panda-kit\" target=\"_blank\">CAN FD panda kit</a> if not using " +
"comma 3X for this <a href=\"https://en.wikipedia.org/wiki/CAN_FD\" target=\"_blank\">CAN FD car</a>.",
Column.MODEL, shop_footnote=False)
@dataclass
class HyundaiCarInfo(CarInfo):
package: str = "Smart Cruise Control (SCC)"
def init_make(self, CP: car.CarParams):
if CP.carFingerprint in CANFD_CAR:
self.footnotes.insert(0, Footnote.CANFD)
CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
CAR.AZERA_6TH_GEN: HyundaiCarInfo("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.AZERA_HEV_6TH_GEN: [
HyundaiCarInfo("Hyundai Azera Hybrid 2019", "All", car_parts=CarParts.common([CarHarness.hyundai_c])),
HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
],
CAR.ELANTRA: [
# TODO: 2017-18 could be Hyundai G
HyundaiCarInfo("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])),
HyundaiCarInfo("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])),
],
CAR.ELANTRA_GT_I30: [
HyundaiCarInfo("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
HyundaiCarInfo("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
],
CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c",
car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.HYUNDAI_GENESIS: [
# TODO: check 2015 packages
HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])),
HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])),
],
CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h])), # TODO: confirm 2020-21 harness
CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", car_parts=CarParts.common([CarHarness.hyundai_b])),
CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", car_parts=CarParts.common([CarHarness.hyundai_g])),
CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o])),
CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_i])), # TODO: check packages
# TODO: this is the 2024 US MY, not yet released
CAR.KONA_EV_2ND_GEN: HyundaiCarInfo("Hyundai Kona Electric (with HDA II, Korea only) 2023", video_link="https://www.youtube.com/watch?v=U2fOCmcQ8hw",
car_parts=CarParts.common([CarHarness.hyundai_r])),
CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", video_link="https://youtu.be/bjDR0YjM__s",
car_parts=CarParts.common([CarHarness.hyundai_d])),
CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4",
car_parts=CarParts.common([CarHarness.hyundai_l])),
CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw",
car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.STARIA_4TH_GEN: HyundaiCarInfo("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", car_parts=CarParts.common([CarHarness.hyundai_e])),
CAR.TUCSON: [
HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_l])),
HyundaiCarInfo("Hyundai Tucson Diesel 2019", car_parts=CarParts.common([CarHarness.hyundai_l])),
],
CAR.PALISADE: [
HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", car_parts=CarParts.common([CarHarness.hyundai_h])),
HyundaiCarInfo("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
],
CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_e])),
CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.IONIQ_5: [
HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_q])),
HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])),
HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])),
],
CAR.IONIQ_6: [
HyundaiCarInfo("Hyundai Ioniq 6 (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])),
],
CAR.TUCSON_4TH_GEN: [
HyundaiCarInfo("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])),
HyundaiCarInfo("Hyundai Tucson 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
],
CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.CUSTIN_1ST_GEN: HyundaiCarInfo("Hyundai Custin 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
# Kia
CAR.KIA_FORTE: [
HyundaiCarInfo("Kia Forte 2019-21", car_parts=CarParts.common([CarHarness.hyundai_g])),
HyundaiCarInfo("Kia Forte 2023", car_parts=CarParts.common([CarHarness.hyundai_e])),
],
CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-24", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_K8_HEV_1ST_GEN: HyundaiCarInfo("Kia K8 Hybrid (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])),
CAR.KIA_NIRO_EV: [
HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])),
HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_f])),
HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_c])),
HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])),
],
CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_NIRO_PHEV: [
HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])),
HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_d])),
],
CAR.KIA_NIRO_PHEV_2022: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_f])),
CAR.KIA_NIRO_HEV_2021: [
HyundaiCarInfo("Kia Niro Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])),
HyundaiCarInfo("Kia Niro Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])),
],
CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control",
car_parts=CarParts.common([CarHarness.hyundai_b])), # TODO: may support 2016, 2018
CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", car_parts=CarParts.common([CarHarness.hyundai_g])),
# TODO: may support adjacent years. may have a non-zero minimum steering speed
CAR.KIA_OPTIMA_H: HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.KIA_OPTIMA_H_G4_FL: HyundaiCarInfo("Kia Optima Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_h])),
CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.KIA_SORENTO: [
HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8",
car_parts=CarParts.common([CarHarness.hyundai_e])),
HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])),
],
CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.KIA_SORENTO_HEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])),
CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0",
car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", car_parts=CarParts.common([CarHarness.hyundai_e])),
CAR.KIA_EV6: [
HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_p])),
HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])),
HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))
],
CAR.KIA_CARNIVAL_4TH_GEN: [
HyundaiCarInfo("Kia Carnival 2022-24", car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarInfo("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k]))
],
# Genesis
CAR.GENESIS_GV60_EV_1ST_GEN: [
HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])),
HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])),
],
CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_f])),
CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_f])),
CAR.GENESIS_GV70_1ST_GEN: [
HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])),
HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_m])),
],
CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", car_parts=CarParts.common([CarHarness.hyundai_c])),
CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_m])),
}
class Buttons:
NONE = 0
RES_ACCEL = 1
SET_DECEL = 2
GAP_DIST = 3
CANCEL = 4 # on newer models, this is a pause/resume button
def get_platform_codes(fw_versions: List[bytes]) -> Set[Tuple[bytes, Optional[bytes]]]:
# Returns unique, platform-specific identification codes for a set of versions
codes = set() # (code-Optional[part], date)
for fw in fw_versions:
code_match = PLATFORM_CODE_FW_PATTERN.search(fw)
part_match = PART_NUMBER_FW_PATTERN.search(fw)
date_match = DATE_FW_PATTERN.search(fw)
if code_match is not None:
code: bytes = code_match.group()
part = part_match.group() if part_match else None
date = date_match.group() if date_match else None
if part is not None:
# part number starts with generic ECU part type, add what is specific to platform
code += b"-" + part[-5:]
codes.add((code, date))
return codes
def match_fw_to_car_fuzzy(live_fw_versions, offline_fw_versions) -> Set[str]:
# Non-electric CAN FD platforms often do not have platform code specifiers needed
# to distinguish between hybrid and ICE. All EVs so far are either exclusively
# electric or specify electric in the platform code.
# TODO: whitelist platforms that we've seen hybrid and ICE versions of that have these specifiers
fuzzy_platform_blacklist = {str(c) for c in set(CANFD_CAR - EV_CAR)}
candidates: Set[str] = set()
for candidate, fws in offline_fw_versions.items():
# Keep track of ECUs which pass all checks (platform codes, within date range)
valid_found_ecus = set()
valid_expected_ecus = {ecu[1:] for ecu in fws if ecu[0] in PLATFORM_CODE_ECUS}
for ecu, expected_versions in fws.items():
addr = ecu[1:]
# Only check ECUs expected to have platform codes
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
# Expected platform codes & dates
codes = get_platform_codes(expected_versions)
expected_platform_codes = {code for code, _ in codes}
expected_dates = {date for _, date in codes if date is not None}
# Found platform codes & dates
codes = get_platform_codes(live_fw_versions.get(addr, set()))
found_platform_codes = {code for code, _ in codes}
found_dates = {date for _, date in codes if date is not None}
# Check platform code + part number matches for any found versions
if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes):
break
if ecu[0] in DATE_FW_ECUS:
# If ECU can have a FW date, require it to exist
# (this excludes candidates in the database without dates)
if not len(expected_dates) or not len(found_dates):
break
# Check any date within range in the database, format is %y%m%d
if not any(min(expected_dates) <= found_date <= max(expected_dates) for found_date in found_dates):
break
valid_found_ecus.add(addr)
# If all live ECUs pass all checks for candidate, add it as a match
if valid_expected_ecus.issubset(valid_found_ecus):
candidates.add(candidate)
return candidates - fuzzy_platform_blacklist
HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf100) # Long description
HYUNDAI_VERSION_REQUEST_ALT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf110) # Alt long description
HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
p16(0xf100)
HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
# Regex patterns for parsing platform code, FW date, and part number from FW versions
PLATFORM_CODE_FW_PATTERN = re.compile(b'((?<=' + HYUNDAI_VERSION_REQUEST_LONG[1:] +
b')[A-Z]{2}[A-Za-z0-9]{0,2})')
DATE_FW_PATTERN = re.compile(b'(?<=[ -])([0-9]{6}$)')
PART_NUMBER_FW_PATTERN = re.compile(b'(?<=[0-9][.,][0-9]{2} )([0-9]{5}[-/]?[A-Z][A-Z0-9]{3}[0-9])')
# List of ECUs expected to have platform codes, camera and radar should exist on all cars
# TODO: use abs, it has the platform code and part number on many platforms
PLATFORM_CODE_ECUS = [Ecu.fwdRadar, Ecu.fwdCamera, Ecu.eps]
# So far we've only seen dates in fwdCamera
# TODO: there are date codes in the ABS firmware versions in hex
DATE_FW_ECUS = [Ecu.fwdCamera]
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
# TODO: minimize shared whitelists for CAN and cornerRadar for CAN-FD
# CAN queries (OBD-II port)
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera],
),
Request(
[HYUNDAI_VERSION_REQUEST_MULTI],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar],
),
# CAN-FD queries (from camera)
# TODO: combine shared whitelists with CAN requests
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac],
bus=0,
auxiliary=True,
),
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar, Ecu.hvac],
bus=1,
auxiliary=True,
obd_multiplexing=False,
),
# CAN-FD debugging queries
Request(
[HYUNDAI_VERSION_REQUEST_ALT],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac],
bus=0,
auxiliary=True,
),
Request(
[HYUNDAI_VERSION_REQUEST_ALT],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac],
bus=1,
auxiliary=True,
obd_multiplexing=False,
),
],
extra_ecus=[
(Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms
(Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms)
(Ecu.hvac, 0x7b3, None), # HVAC Control Assembly
(Ecu.cornerRadar, 0x7b7, None),
],
# Custom fuzzy fingerprinting function using platform codes, part numbers + FW dates:
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
CHECKSUM = {
"crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021,
CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022,
CAR.KIA_K5_HEV_2020, CAR.CUSTIN_1ST_GEN],
"6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
}
CAN_GEARS = {
# which message has the gear. hybrid and EV use ELECT_GEAR
"use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
"use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON},
}
CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN,
CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN,
CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN,
CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KONA_EV_2ND_GEN, CAR.KIA_K8_HEV_1ST_GEN,
CAR.STARIA_4TH_GEN}
# The radar does SCC on these cars when HDA I, rather than the camera
CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80,
CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN}
# These CAN FD cars do not accept communication control to disable the ADAS ECU,
# responds with 0x7F2822 - 'conditions not correct'
CANFD_UNSUPPORTED_LONGITUDINAL_CAR = {CAR.IONIQ_6, CAR.KONA_EV_2ND_GEN}
# The camera does SCC on these cars, rather than the radar
CAMERA_SCC_CAR = {CAR.KONA_EV_2022, }
# these cars use a different gas signal
HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ,
CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN,
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.KIA_K5_HEV_2020, CAR.KIA_NIRO_HEV_2ND_GEN,
CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_K8_HEV_1ST_GEN,
CAR.AZERA_HEV_6TH_GEN, CAR.KIA_NIRO_PHEV_2022}
EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022,
CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KONA_EV_2ND_GEN}
# these cars require a special panda safety mode due to missing counters and checksums in the messages
LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.KIA_OPTIMA_G4,
CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022,
CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
# these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc.
UNSUPPORTED_LONGITUDINAL_CAR = LEGACY_SAFETY_MODE_CAR | {CAR.KIA_NIRO_PHEV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4_FL,
CAR.KIA_OPTIMA_H_G4_FL}
# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
# If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py
DBC = {
CAR.AZERA_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
CAR.AZERA_HEV_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G70_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None),
CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_PHEV_2019: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.IONIQ: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_K5_HEV_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_H_G4_FL: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_STINGER_2022: dbc_dict('hyundai_kia_generic', None),
CAR.KONA: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None),
CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
CAR.TUCSON: dbc_dict('hyundai_kia_generic', None),
CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_EV6: dbc_dict('hyundai_canfd', None),
CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.TUCSON_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.TUCSON_HYBRID_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.IONIQ_5: dbc_dict('hyundai_canfd', None),
CAR.IONIQ_6: dbc_dict('hyundai_canfd', None),
CAR.SANTA_CRUZ_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SPORTAGE_5TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.GENESIS_GV70_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SORENTO_PHEV_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SORENTO_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_NIRO_EV_2ND_GEN: dbc_dict('hyundai_canfd', None),
CAR.GENESIS_GV80: dbc_dict('hyundai_canfd', None),
CAR.KIA_CARNIVAL_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_SORENTO_HEV_4TH_GEN: dbc_dict('hyundai_canfd', None),
CAR.KONA_EV_2ND_GEN: dbc_dict('hyundai_canfd', None),
CAR.KIA_K8_HEV_1ST_GEN: dbc_dict('hyundai_canfd', None),
CAR.CUSTIN_1ST_GEN: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_NIRO_PHEV_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.STARIA_4TH_GEN: dbc_dict('hyundai_canfd', None),
}

332
selfdrive/car/hyundai/carcontroller.2 Normal file
View File

@@ -0,0 +1,332 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR, LEGACY_SAFETY_MODE_CAR
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
from openpilot.common.params import Params
from openpilot.common.watcher import Watcher
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
params_memory = Params("/dev/shm/params")
# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
# All slightly below EPS thresholds to avoid fault
MAX_ANGLE = 85
MAX_ANGLE_FRAMES = 89
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
# initialize to no line visible
# TODO: this is not accurate for all cars
sys_state = 1
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
sys_state = 3 if enabled or sys_warning else 4
elif hud_control.leftLaneVisible:
sys_state = 5
elif hud_control.rightLaneVisible:
sys_state = 6
# initialize to no warnings
left_lane_warning = 0
right_lane_warning = 0
if hud_control.leftLaneDepart:
left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
if hud_control.rightLaneDepart:
right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
return sys_warning, sys_state, left_lane_warning, right_lane_warning
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.CAN = CanBus(CP)
self.params = CarControllerParams(CP)
self.packer = CANPacker(dbc_name)
self.angle_limit_counter = 0
self.frame = 0
self.accel_last = 0
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.last_button_frame = 0
self.last_resume_frame = 0
self.last_debug_frame = 0
def update(self, CC, CS, now_nanos, sport_plus):
actuators = CC.actuators
hud_control = CC.hudControl
# hud_v_cruise = hud_control.setSpeed
# if hud_v_cruise > 70:
# hud_v_cruise = 0
# steering torque
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
# >90 degree steering fault prevention
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
self.angle_limit_counter, MAX_ANGLE_FRAMES,
MAX_ANGLE_CONSECUTIVE_FRAMES)
if not CC.latActive:
apply_steer = 0
# Hold torque with induced temporary fault when cutting the actuation bit
torque_fault = CC.latActive and not apply_steer_req
self.apply_steer_last = apply_steer
# accel + longitudinal
if sport_plus:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS)
else:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
can_sends = []
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, self.CAN.ECAN
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
# CAN-FD platforms
if self.CP.carFingerprint in CANFD_CAR:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
# MODIFIED BBOT
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING))
# MODIFIED BBOT
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
# can_sends.append(hyundaicanfd.create_misc_messages(self.packer, self.CAN, self.frame))
if self.CP.openpilotLongitudinalControl:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, CS.personality_profile))
self.accel_last = accel
else:
# button presses
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False, set_speed_in_units = None))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, CS.out.cruiseState.available, CS.personality_profile))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(hyundaican.create_acc_opt(self.packer))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
# CSLC
# if not self.CP.openpilotLongitudinalControl and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
# cslcSetSpeed = set_speed_in_units
# self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
# if self.cruise_button != Buttons.NONE:
# # if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# # send_freq = 1
# # # send resume at a max freq of 10Hz
# # if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# # can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# # if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# # self.last_button_frame = self.frame
# if self.frame % 2 == 0:
# if self.CP.carFingerprint in CANFD_CAR:
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
# else:
# can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# Test - Works???
# if CS.cruise_buttons == Buttons.NONE and CS.cruiseState.enabled:
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, Buttons.SET_DECEL))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
Watcher.log_watch("hyundai_carcontroller_update_CC", CC)
Watcher.log_watch("hyundai_carcontroller_update_CS", CS)
Watcher.log_watch("hyundai_carcontroller_update_self", self)
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool, set_speed_in_units = None):
can_sends = []
# Test me.
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, 1, Buttons.RES_ACCEL))
# if self.CP.openpilotLongitudinalControl:
# CC.cruiseControl.resume = True
# self.CP.openpilotLongitudinalControl = False
# else:
# CC.cruiseControl.cancel = True
# self.CP.openpilotLongitudinalControl = True
if use_clu11:
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint))
elif CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likeli M,hood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
else:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# if (self.frame - self.last_button_frame) * DT_CTRL > 3:
# if self.last_resume_frame = self.frame
if CS.oscar_lane_center_btn_pressed:
CS.oscar_lane_center_btn_pressed = False
# CC.cruiseControl.resume = True
CC.cruiseControl.cancel = True
# Test this...
# Also try create_acc_commands
# This attempts to set the speed to
# stopping = CC.actuators.longControlState == LongCtrlState.stopping
# can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, CC.actuators.accel, stopping, CC.cruiseControl.override,
# 40, CS.personality_profile))
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if (self.frame - self.last_button_frame) * DT_CTRL > 4:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# for _ in range(20):
nothing = 0
# Nothing for now --?
# oscar - test me
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
self.last_resume_frame = self.frame
elif set_speed_in_units is not None and not self.CP.openpilotLongitudinalControl and CS.cruiseState.enabled and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
cslcSetSpeed = set_speed_in_units
self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
if self.cruise_button != Buttons.NONE:
# if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# send_freq = 1
# # send resume at a max freq of 10Hz
# if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# self.last_button_frame = self.frame
if self.frame % 2 == 0:
for _ in range(20):
if self.CP.carFingerprint in CANFD_CAR:
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
else:
can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
self.last_button_frame = self.frame
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25)
return can_sends
def get_set_speed(self, hud_v_cruise):
v_cruise_kph = min(hud_v_cruise * CV.MS_TO_KPH, V_CRUISE_MAX)
v_cruise = int(round(v_cruise_kph * CV.KPH_TO_MPH))
v_cruise_slc: int = 0
# v_cruise_slc = params_memory.get_int("CSLCSpeed")
if v_cruise_slc > 0:
v_cruise = v_cruise_slc
return v_cruise
def get_cslc_button(self, cslcSetSpeed, CS, CC):
cruiseBtn = Buttons.NONE
speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH))
if cslcSetSpeed < speedSetPoint and speedSetPoint > 25 and CC.enabled and CC.experimental_mode:
cruiseBtn = Buttons.SET_DECEL
elif cslcSetSpeed > speedSetPoint and speedSetPoint < 85 and CC.enabled:
cruiseBtn = Buttons.RES_ACCEL
else:
cruiseBtn = Buttons.SET_DECEL
# cruiseBtn = Buttons.NONE
return cruiseBtn

512
selfdrive/car/hyundai/carcontroller.py
View File

@@ -12,6 +12,18 @@ from openpilot.common.params import Params
from openpilot.common.watcher import Watcher
# DEVELOPOMENT TODO
# - Write a loop that runs and does nothing.
# - Load as many inputs in possible into state variables.
# - Make a state variable tester. It should be a web server running async.
# - Make a button tester, also on web server, to test engaging each button.
# - Write basic interface refactor
# - Write experimental mode speed controller
# - Write resume from stop controller
# - Implement as many extra features as possible (auto hvac, auto sunroof, suggest a break, remind headlights)
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -23,7 +35,148 @@ MAX_ANGLE = 85
MAX_ANGLE_FRAMES = 89
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
# Constants for state arrays.
ENGAGED = 'engaged'
STANDBY = 'standby'
OFF = 'off'
# Input
# Current state of openpilot and its desired commands
openpilot_state = {
'openpilot_ready': False, # Openpilot is enabled in settings
'lateral_active': False, # Lateral control is engaged either full or always on lateral
'openpilot_engaged': False, # Openpilot is full engaged (lateral + cruise control)
'always_on_lateral_active': False, # Cruise control disengaged but lateral still engaged
'experimental_active': False, # Frogpilot has engaged experimental control, so we need to adjust speed and possibly apply brakes (if this is possible)
'experimental_desired_speed': 0, # The speed experimental control wants to set long to
'curviture_angle': 0, # Detected current curviture (ideally within the next 3 or 4 seconds) for taking over LKAS and possibly LONG
'lane_change_enabled': False, # The lane change assist feature is active
'lane_change_active': False, # Executing a lane change assist
'connected_to_internet': False,
'rate_limited_internet': False
}
# Input
# Esoteric things related to the car that support other features than driving. Will be shown in debugger.
car_state = {
'location_lat': 0,
'location_long': 0,
'drive_distance_this_trip': 0,
'drive_distance_today': 0,
'compass_direction': 'N', # Unclear if I will use this
'fuel': 1, # 0-1 fuel tank level
'windows_down': False, # True if any windows / sunroof is open
'climate_control': False,
'climate_control_set_temp': 72,
'cabin_temprature': 50,
'heated_seat_driver_on': False,
'heated_seat_passenger_on': False,
'heated_steering_wheel_on': False,
'fan_seat_driver_on': False,
'fan_seat_passenger_on': False,
'info_panel_item_showing': 0, # If we can capture what instrument panel info is showing, we can change it to speed
'daytime': False,
'headlights': False, # Would be cool to remind to have the headlights turned on if moving
}
# Input
# The current status of cruise control activation
cruise_control_state = {
'set_speed': 0, # Cruise control set speed
'actual_speed': 0, # Spedometer reading
'speed_limit': 0, # Speed limit as reported by car's dashboard
'cruise_control_status': OFF, # or ENGAGED or STANDBY
'brake_pressed': False
}
# Input
# The current state of the car in front of us, if cruise control is engaged but the car is not moving
stationary_state = {
'stationary': False, # When speed reaches 0 we are stationary
'stationary_since': 0, # At what time stationary was achieved
'stationary_has_lead': False, # At the time of becoming stationary, was there a lead veichele?
'stationary_has_lead_distance': 0, # At the time of becoming stationary, what was the distance to the lead veichele?
'current_lead_car_distance': 0, # What is the distance to the lead veichele now?
'lead_vehicle_moving_away': False, # Has logic decided that the current veichele is moving away?
'oem_lanes_detected': False # Is the car reporting it can see lanes? I would like to experiment with engaging stock LKAS.
}
# Input
# What buttons the user is pressing. Can be None, BUTTON_SHORT, or BUTTON_LONG (held for 1/2 second)
cruise_control_buttons_input = {
'engage_cruise_control': None,
'cruise_control_speed_up': None,
'cruise_control_speed_down': None,
'pause_resume_cruise_control': None,
'lane_keep_assist_button': None,
'lane_follow_assist_button': None
}
# Output
# What buttons we wish to simulate pressing.
cruise_control_buttons_output = {
'engage_cruise_control': False,
'cruise_control_speed_up': False,
'cruise_control_speed_down': False,
'pause_resume_cruise_control': False,
'lane_keep_assist_button': False,
'lane_follow_assist_button': False,
}
# Output
# What buttons we wish to simulate pressing.
# I don't know if I will have access to these.
other_buttons_output = {
'info_category': False,
'info_section': False,
'heated_seat_driver': False,
'heated_seat_passenger': False,
'heated_steering_wheel': False,
'fan_seat_driver': False,
'fan_seat_passenger': False,
'sunroof': False,
'hvac_power': False,
'hvac_sync': False,
'hvac_up': False,
'hvac_down': False
}
# What to display on the instrument panel. Sent every frame.
instrument_panel_outputs = {
'lane_keeping_assist_active': False,
'forward_collision_warning_active': False,
'high_beam_assist_active': False,
'forward_collision_avoidance_assist_active': False,
'blind_spot_collision_warning_active': False,
'lane_following_assist_active': False,
'driver_attention_warning_active': False,
'lane_departure_warning_active': False
}
# State
# Variables to save what oscarpilot is doing - behaviors will change depending
# on current state
oscarpilot_state = [
'set_speed': 0, # Current desired cruise speed
'speed_override': False, # Set if moving away from set speed due to experimental
'speed_restore': False, # Set if moving twoards set speed due to reengagement or change speed to speed limit
'lkas_oem': False, # Set if allowing the car oem software to control LKAS
'daytime': False, # Set if it is daytime. Car is more judgmental at night.
]
# Output
# These are messages to the user, and get set to false when processed by openpilot
# Ideally we can set these in the infotainment area as well, like carplay audio selection
oscarpilot_alerts = {
'detected_stop_sign_or_light': False, # Openpilot has seen a stoplight or stopsign
'very_sharp_curve': False, # The upcoming curve is probably too sharp to be handled automatically
'lane_confidence_low': False, # The confidence level that openpilot knows what its doing is low
'heavy_traffic_ahead': False, # Mapping data indicates heavy traffic within 2 miles
'weather_ahead': False, # Mapping data indicates precipitation within 5 miles
'suggest_a_break': False, # Variable will be used to suggest a break (3 hours at day, 1.5 hours at night)
}
# Where is this called? Public? Private? Should we refactor?
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
@@ -47,7 +200,6 @@ def process_hud_alert(enabled, fingerprint, hud_control):
return sys_warning, sys_state, left_lane_warning, right_lane_warning
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
@@ -64,28 +216,90 @@ class CarController:
self.last_resume_frame = 0
self.last_debug_frame = 0
self._CC = None
# self._CC.actuators
# self._CC.hudControl
self._CS = None
self._now_nanos = None
self._sport_plus = None
self._actuators = None
self._hud_control = None
def update(self, CC, CS, now_nanos, sport_plus):
actuators = CC.actuators
hud_control = CC.hudControl
self._CC = CC
self._CS = CS
self._now_nanos = now_nanos
self._sport_plus = sport_plus
# hud_v_cruise = hud_control.setSpeed
# if hud_v_cruise > 70:
# hud_v_cruise = 0
self._apply_steer = None
self._accel = None
can_sends = []
# CAN-FD car logic
if self.CP.carFingerprint in CANFD_CAR:
# Steering control for CAN FD
# can_sends.extend(self._create_common_messages())
can_sends.extend(self._create_steering_messages())
can_sends.extend(self._create_instrument_messages())
can_sends.extend(self._create_button_messages())
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
self.frame += 1
return new_actuators, can_sends
# def _create_common_messages():
# *** common hyundai stuff ***
def _unprocessed():
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
can_sends = []
# CAN-FD platforms
# if self.CP.carFingerprint in CANFD_CAR:
# # LFA and HDA icons
# if self.frame % 5 == 0 and (not hda2 or hda2_long):
# can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# if self.CP.openpilotLongitudinalControl:
# if hda2:
# can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
# if self.frame % 2 == 0:
# can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
# set_speed_in_units, CS.personality_profile))
# self.accel_last = accel
# else:
# # button presses
# can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
# Mostly unchanged from frogpilot.
def _create_steering_messages():
can_sends = []
# steering torque
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, self._CS.out.steeringTorque, self.params)
# >90 degree steering fault prevention
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(self._CS.out.steeringAngleDeg) >= MAX_ANGLE, self._CC.latActive,
self.angle_limit_counter, MAX_ANGLE_FRAMES,
MAX_ANGLE_CONSECUTIVE_FRAMES)
if not CC.latActive:
apply_steer = 0
# Hold torque with induced temporary fault when cutting the actuation bit
torque_fault = CC.latActive and not apply_steer_req
torque_fault = self._CC.latActive and not apply_steer_req
self.apply_steer_last = apply_steer
@@ -94,240 +308,70 @@ class CarController:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS)
else:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
can_sends = []
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, self.CAN.ECAN
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
# CAN-FD platforms
if self.CP.carFingerprint in CANFD_CAR:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
# MODIFIED BBOT
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING))
# MODIFIED BBOT
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
# can_sends.append(hyundaicanfd.create_misc_messages(self.packer, self.CAN, self.frame))
if self.CP.openpilotLongitudinalControl:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, CS.personality_profile))
self.accel_last = accel
else:
# button presses
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False, set_speed_in_units = None))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, CS.out.cruiseState.available, CS.personality_profile))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(hyundaican.create_acc_opt(self.packer))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
# CSLC
# if not self.CP.openpilotLongitudinalControl and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
# cslcSetSpeed = set_speed_in_units
# self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
# if self.cruise_button != Buttons.NONE:
# # if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# # send_freq = 1
# # # send resume at a max freq of 10Hz
# # if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# # can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# # if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# # self.last_button_frame = self.frame
# if self.frame % 2 == 0:
# if self.CP.carFingerprint in CANFD_CAR:
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
# else:
# can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# Test - Works???
# if CS.cruise_buttons == Buttons.NONE and CS.cruiseState.enabled:
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, Buttons.SET_DECEL))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
Watcher.log_watch("hyundai_carcontroller_update_CC", CC)
Watcher.log_watch("hyundai_carcontroller_update_CS", CS)
Watcher.log_watch("hyundai_carcontroller_update_self", self)
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool, set_speed_in_units = None):
can_sends = []
# Test me.
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, 1, Buttons.RES_ACCEL))
# if self.CP.openpilotLongitudinalControl:
# CC.cruiseControl.resume = True
# self.CP.openpilotLongitudinalControl = False
# else:
# CC.cruiseControl.cancel = True
# self.CP.openpilotLongitudinalControl = True
if use_clu11:
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint))
elif CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likeli M,hood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
else:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# if (self.frame - self.last_button_frame) * DT_CTRL > 3:
# if self.last_resume_frame = self.frame
if CS.oscar_lane_center_btn_pressed:
CS.oscar_lane_center_btn_pressed = False
# CC.cruiseControl.resume = True
CC.cruiseControl.cancel = True
# Test this...
# Also try create_acc_commands
# This attempts to set the speed to
# stopping = CC.actuators.longControlState == LongCtrlState.stopping
# can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, CC.actuators.accel, stopping, CC.cruiseControl.override,
# 40, CS.personality_profile))
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if (self.frame - self.last_button_frame) * DT_CTRL > 4:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# for _ in range(20):
nothing = 0
# Nothing for now --?
# oscar - test me
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
self.last_resume_frame = self.frame
elif set_speed_in_units is not None and not self.CP.openpilotLongitudinalControl and CS.cruiseState.enabled and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
cslcSetSpeed = set_speed_in_units
self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
if self.cruise_button != Buttons.NONE:
# if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# send_freq = 1
# # send resume at a max freq of 10Hz
# if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# self.last_button_frame = self.frame
if self.frame % 2 == 0:
for _ in range(20):
if self.CP.carFingerprint in CANFD_CAR:
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
else:
can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
self.last_button_frame = self.frame
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25)
# blinkers
# if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
# can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
self._apply_steer = apply_steer
self._accel = accel
return can_sends
# Placeholder for potential instrument panel processing
def _create_instrument_messages(instrument_panel_state):
can_sends = []
# Assume default values or read the initial state
lfa_icon_state = 0
hda_active = 0
hda_icon_state = 0
hda_set_speed = 0 # This would likely need to be more dynamic based on context
# Assign values based on instrument_panel_outputs states
for output in instrument_panel_outputs:
if output['name'] == 'lane_keeping_assist_active':
lfa_icon_state = 2 if output['value'] else 0
elif output['name'] == 'hda_active': # Hypothetical key for example
hda_active = 1 if output['value'] else 0
elif output['name'] == 'hda_icon_state': # Also hypothetical
hda_icon_state = 2 if output['value'] else 0
# Add other conditions based on the actual data structure you expect
# Assume we can bundle all these states into one message as in your example
values = {
"LFA_Icon_State": lfa_icon_state,
"HDA_Active": hda_active,
"HDA_Icon_State": hda_icon_state,
"HDA_VSetReq": hda_set_speed, # This example assumes static speed; adjust if dynamic
}
can_sends.append(packer.make_can_msg("LFAHDA_MFC", 0, values))
def get_set_speed(self, hud_v_cruise):
v_cruise_kph = min(hud_v_cruise * CV.MS_TO_KPH, V_CRUISE_MAX)
v_cruise = int(round(v_cruise_kph * CV.KPH_TO_MPH))
return can_sends
v_cruise_slc: int = 0
# v_cruise_slc = params_memory.get_int("CSLCSpeed")
# Placeholder for potential button press processing
def _create_button_messages(button_presses):
can_sends = []
if v_cruise_slc > 0:
v_cruise = v_cruise_slc
return v_cruise
for button in cruise_control_buttons:
if button['value']:
# Resetting button state to prevent repeated presses
button['value'] = False
def get_cslc_button(self, cslcSetSpeed, CS, CC):
cruiseBtn = Buttons.NONE
speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH))
if button['name'] in button_map:
# Mapping each button to a specific CAN message
btn_value = button_map[button['name']]
can_sends.append(create_buttons(packer, CP, CAN, 0, btn_value)) # Counter is example, replace with actual logic if necessary
if cslcSetSpeed < speedSetPoint and speedSetPoint > 25 and CC.enabled and CC.experimental_mode:
cruiseBtn = Buttons.SET_DECEL
elif cslcSetSpeed > speedSetPoint and speedSetPoint < 85 and CC.enabled:
cruiseBtn = Buttons.RES_ACCEL
else:
cruiseBtn = Buttons.SET_DECEL
# cruiseBtn = Buttons.NONE
return cruiseBtn
return can_sends

500
selfdrive/car/hyundai/carstate.org Normal file
View File

@@ -0,0 +1,500 @@
from collections import deque
import copy
import math
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, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
from openpilot.common.watcher import Watcher
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.carFingerprint in EV_CAR else \
"ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR 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)
def update(self, cp, cp_cam):
Watcher.log_watch("hyundai_carstate_update_cp", cp)
Watcher.log_watch("hyundai_carstate_update_cp_cam", cp_cam)
if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam)
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.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
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.carFingerprint in (HYBRID_CAR | EV_CAR):
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
# BBot functions for lfa and gap buttons - test speed up / down
self.oscar_lane_center_btn_pressed= False
self.oscar_slc_decel = False
self.oscar_slc_accel = False
self.param_memory.put("oscar_debug", "Hello World")
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# self.custom_speed_up = True
# self.lkas_previously_pressed = lkas_pressed
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
Watcher.log_watch("hyundai_carstate_update_self", self)
return ret
def update_canfd(self, cp, cp_cam):
Watcher.log_watch("hyundai_carstate_update_canfd_cp", cp)
Watcher.log_watch("hyundai_carstate_update_canfd_cp_cam", cp_cam)
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.carFingerprint in (EV_CAR | HYBRID_CAR):
offset = 255. if self.CP.carFingerprint in EV_CAR 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.carFingerprint in EV_CAR:
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
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"])
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
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"])
SpeedLimitController.load_state()
SpeedLimitController.car_speed_limit = self.calculate_speed_limit_canfd(self.CP, cp, cp_cam) * speed_factor
SpeedLimitController.write_car_state()
# self.custom_speed_up = False
self.oscar_lane_center_btn_pressed = False
if ret.cruiseState.available:
if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
self.oscar_lane_center_btn_pressed = True
lkas_pressed = False
try:
lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
except:
nothing = 0
# intentionally cause a failure
if lkas_pressed:
floog=norpdywoop
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
Watcher.log_watch("hyundai_carstate_update_canfd_self", self)
return ret
# BBOT does not work
def calculate_speed_limit_canfd(self, CP, cp, cp_cam):
try:
self._speed_limit_clu = cp.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
return self._speed_limit_clu if self._speed_limit_clu not in (0, 255) else 0
except:
return 0
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.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("E_EMS11", 50))
else:
messages += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
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))
messages.append(("BCM_PO_11", 50))
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.carFingerprint in EV_CAR:
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 CP.nav_msg:
# messages.append(("CLUSTER_SPEED_LIMIT", 10))
if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl:
messages += [
("SCC_CONTROL", 50),
]
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),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)

408
selfdrive/car/hyundai/carstate.py
View File

@@ -27,32 +27,6 @@ class CarState(CarStateBase):
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.carFingerprint in EV_CAR else \
"ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR 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)
def update(self, cp, cp_cam):
@@ -62,391 +36,15 @@ class CarState(CarStateBase):
if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam)
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.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
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.carFingerprint in (HYBRID_CAR | EV_CAR):
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
# BBot functions for lfa and gap buttons - test speed up / down
self.oscar_lane_center_btn_pressed= False
self.oscar_slc_decel = False
self.oscar_slc_accel = False
self.param_memory.put("oscar_debug", "Hello World")
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# self.custom_speed_up = True
# self.lkas_previously_pressed = lkas_pressed
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
Watcher.log_watch("hyundai_carstate_update_self", self)
return ret
def update_canfd(self, cp, cp_cam):
Watcher.log_watch("hyundai_carstate_update_canfd_cp", cp)
Watcher.log_watch("hyundai_carstate_update_canfd_cp_cam", cp_cam)
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.carFingerprint in (EV_CAR | HYBRID_CAR):
offset = 255. if self.CP.carFingerprint in EV_CAR 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.carFingerprint in EV_CAR:
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
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"])
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
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"])
SpeedLimitController.load_state()
SpeedLimitController.car_speed_limit = self.calculate_speed_limit_canfd(self.CP, cp, cp_cam) * speed_factor
SpeedLimitController.write_car_state()
# self.custom_speed_up = False
self.oscar_lane_center_btn_pressed = False
if ret.cruiseState.available:
if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
self.oscar_lane_center_btn_pressed = True
lkas_pressed = False
try:
lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
except:
nothing = 0
# intentionally cause a failure
if lkas_pressed:
floog=norpdywoop
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
Watcher.log_watch("hyundai_carstate_update_canfd_self", self)
return ret
# BBOT does not work
def calculate_speed_limit_canfd(self, CP, cp, cp_cam):
try:
self._speed_limit_clu = cp.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
return self._speed_limit_clu if self._speed_limit_clu not in (0, 255) else 0
except:
return 0
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.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("E_EMS11", 50))
else:
messages += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
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))
messages.append(("BCM_PO_11", 50))
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),
@@ -485,7 +83,7 @@ class CarState(CarStateBase):
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
@staticmethod
def get_cam_can_parser_canfd(CP):
def get_cam_can_parser(CP):
messages = []
if CP.flags & HyundaiFlags.CANFD_HDA2:
block_lfa_msg = "CAM_0x362" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "CAM_0x2a4"
@@ -495,6 +93,4 @@ class CarState(CarStateBase):
("SCC_CONTROL", 50),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)

224
selfdrive/car/hyundai/hyundaicanfd.org Normal file
View File

@@ -0,0 +1,224 @@
from openpilot.common.numpy_fast import clip
from openpilot.selfdrive.car import CanBusBase
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
class CanBus(CanBusBase):
def __init__(self, CP, hda2=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
if hda2 is None:
assert CP is not None
hda2 = CP.flags & HyundaiFlags.CANFD_HDA2.value
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars
# have a different harness than the HDA1 and non-HDA variants in order to split
# a different bus, since the steering is done by different ECUs.
self._a, self._e = 1, 0
if hda2:
self._a, self._e = 0, 1
self._a += self.offset
self._e += self.offset
self._cam = 2 + self.offset
@property
def ECAN(self):
return self._e
@property
def ACAN(self):
return self._a
@property
def CAM(self):
return self._cam
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
ret = []
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1 if lat_active else 0,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
"STEER_MODE": 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
"NEW_SIGNAL_1": 0,
"NEW_SIGNAL_2": 0,
}
if CP.flags & HyundaiFlags.CANFD_HDA2:
hda2_lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "LKAS"
if CP.openpilotLongitudinalControl:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
ret.append(packer.make_can_msg(hda2_lkas_msg, CAN.ACAN, values))
else:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
return ret
def create_suppress_lfa(packer, CAN, hda2_lfa_block_msg, hda2_alt_steering):
suppress_msg = "CAM_0x362" if hda2_alt_steering else "CAM_0x2a4"
msg_bytes = 32 if hda2_alt_steering else 24
values = {f"BYTE{i}": hda2_lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7}
values["COUNTER"] = hda2_lfa_block_msg["COUNTER"]
values["SET_ME_0"] = 0
values["SET_ME_0_2"] = 0
values["LEFT_LANE_LINE"] = 0
values["RIGHT_LANE_LINE"] = 0
return packer.make_can_msg(suppress_msg, CAN.ACAN, values)
def create_buttons(packer, CP, CAN, cnt, btn):
values = {
"COUNTER": cnt,
"SET_ME_1": 1,
"CRUISE_BUTTONS": btn,
}
bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
# TODO: why do we copy different values here?
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"NEW_SIGNAL_1",
"MainMode_ACC",
"ACCMode",
"ZEROS_9",
"CRUISE_STANDSTILL",
"ZEROS_5",
"DISTANCE_SETTING",
"VSetDis",
]}
else:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"ACCMode",
"VSetDis",
"CRUISE_STANDSTILL",
]}
values.update({
"ACCMode": 4,
"aReqRaw": 0.0,
"aReqValue": 0.0,
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CAN, enabled):
values = {
"HDA_ICON": 1 if enabled else 0,
# 0 off, 1 gray, 2 green, 3 blinking (wheel icon)
"LFA_ICON": 0 if enabled else 0, # was green before i think in 2? 3 should be flashing?
# "LFA_ICON": 2 if enabled else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, personality):
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
a_val, a_raw = 0, 0
else:
a_raw = accel
a_val = clip(accel, accel_last - jn, accel_last + jn)
values = {
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
"MainMode_ACC": 1,
"StopReq": 1 if stopping else 0,
"aReqValue": a_val,
"aReqRaw": a_raw,
"VSetDis": set_speed,
"JerkLowerLimit": jerk if enabled else 1,
"JerkUpperLimit": 3.0,
"ACC_ObjDist": 1,
"ObjValid": 0,
"OBJ_STATUS": 2,
"SET_ME_2": 0x4,
"SET_ME_3": 0x3,
"SET_ME_TMP_64": 0x64,
"DISTANCE_SETTING": personality + 1,
}
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
ret = []
values = {
}
ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values))
blink = 0
if left_blink:
blink = 3
elif right_blink:
blink = 4
values = {
"BLINKER_CONTROL": blink,
}
ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values))
return ret
def create_adrv_messages(packer, CAN, frame):
# messages needed to car happy after disabling
# the ADAS Driving ECU to do longitudinal control
ret = []
values = {
}
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
if frame % 2 == 0:
values = {
'AEB_SETTING': 0x1, # show AEB disabled icon
'SET_ME_2': 0x2,
'SET_ME_FF': 0xff,
'SET_ME_FC': 0xfc,
'SET_ME_9': 0x9,
}
ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
if frame % 5 == 0:
values = {
'SET_ME_1C': 0x1c,
'SET_ME_FF': 0xff,
'SET_ME_TMP_F': 0xf,
'SET_ME_TMP_F_2': 0xf,
}
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))
values = {
'SET_ME_E1': 0xe1,
'SET_ME_3A': 0x3a,
}
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values))
if frame % 20 == 0:
values = {
'SET_ME_15': 0x15,
}
ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values))
if frame % 100 == 0:
values = {
'SET_ME_22': 0x22,
'SET_ME_41': 0x41,
}
ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values))
return ret

3
selfdrive/oscarpilot/settings/basic.cc
View File

@@ -21,11 +21,10 @@ OscarPilotBasicPanel::OscarPilotBasicPanel(OscarSettingsWindow *parent) : FrogPi
// "Engages 'experimental mode' when a curve is detected, temporairly reducing max speed.", "../frogpilot/assets/toggle_icons/icon_conditional.png"
// },
// Alert on stopsign / stoplight
{"LaneChangeAssist", "Lane Change Assist", "Automatically change lanes on turn signal and wheel nudge at highway speeds.", "../frogpilot/assets/toggle_icons/icon_lane.png"},
{"FireTheBabysitter", "Custom Driver Monitoring", "Customize the driver awareness alert timeouts.", ""},
// - Hands On Wheel: Always / At Dusk / 2+ hrs driving
{"LaneChangeAssist", "Lane Change Assist", "Automatically change lanes on turn signal and wheel nudge at highway speeds.", "../frogpilot/assets/toggle_icons/icon_lane.png"},
{"DashCam", "Dash Cam Recording", "Record video and gps data for all drives automatically.", ""}
};

21
selfdrive/oscarpilot/settings/notes
View File

@@ -1 +1,20 @@
Need a reset to defaults option
Goals:
- slow down on experimental mode
- LKAS button - should toggle between drive, weather, weather radar. Hold for screen off
- car dashboard should have enough indicators that it is able to represent drive state without screen on
- set speed limit on engage cruise control
qol:
- customize babysitter - driver awareness timeouts daytime, nighttime, require steering wheel
- set climate on start, roll up windows on stop
- upload park location on stop
- low bandwidth / high bandwidth wifi, upload recordings on high bandwidth
- list drives w/ image on stop
- stop light / stop sign warning alert
release:
- reenable training screen
- - disregard for my device id
- cleanup code
- rename to diamondpilot