@ -3,8 +3,8 @@ from common.realtime import DT_CTRL
from common . numpy_fast import clip , interp
from common . numpy_fast import clip , interp
from common . conversions import Conversions as CV
from common . conversions import Conversions as CV
from selfdrive . car import apply_std_steer_torque_limits
from selfdrive . car import apply_std_steer_torque_limits
from selfdrive . car . hyundai . hyundaican import create_lkas11 , create_clu11 , create_lfahda_mfc , create_acc_commands , create_acc_opt , create_frt_radar_opt
from selfdrive . car . hyundai import hda2can , hyundaican
from selfdrive . car . hyundai . values import Buttons , CarControllerParams , CAR
from selfdrive . car . hyundai . values import Buttons , CarControllerParams , HDA2_CAR , CAR
from opendbc . can . packer import CANPacker
from opendbc . can . packer import CANPacker
VisualAlert = car . CarControl . HUDControl . VisualAlert
VisualAlert = car . CarControl . HUDControl . VisualAlert
@ -44,13 +44,12 @@ class CarController:
self . apply_steer_last = 0
self . apply_steer_last = 0
self . car_fingerprint = CP . carFingerprint
self . car_fingerprint = CP . carFingerprint
self . steer_rate_limited = False
self . steer_rate_limited = False
self . last_resume _frame = 0
self . last_button _frame = 0
self . accel = 0
self . accel = 0
def update ( self , CC , CS ) :
def update ( self , CC , CS ) :
actuators = CC . actuators
actuators = CC . actuators
hud_control = CC . hudControl
hud_control = CC . hudControl
pcm_cancel_cmd = CC . cruiseControl . cancel
# Steering Torque
# Steering Torque
new_steer = int ( round ( actuators . steer * self . params . STEER_MAX ) )
new_steer = int ( round ( actuators . steer * self . params . STEER_MAX ) )
@ -67,25 +66,42 @@ class CarController:
can_sends = [ ]
can_sends = [ ]
if self . CP . carFingerprint in HDA2_CAR :
# steering control
can_sends . append ( hda2can . create_lkas ( self . packer , CC . enabled , self . frame , CC . latActive , apply_steer ) )
# cruise cancel
if ( self . frame - self . last_button_frame ) * DT_CTRL > 0.25 :
if CC . cruiseControl . cancel :
for _ in range ( 20 ) :
can_sends . append ( hda2can . create_buttons ( self . packer , CS . buttons_counter + 1 , True , False ) )
self . last_button_frame = self . frame
# cruise standstill resume
elif CC . enabled and CS . out . cruiseState . standstill :
can_sends . append ( hda2can . create_buttons ( self . packer , CS . buttons_counter + 1 , False , True ) )
self . last_button_frame = self . frame
else :
# tester present - w/ no response (keeps radar disabled)
# tester present - w/ no response (keeps radar disabled)
if self . CP . openpilotLongitudinalControl :
if self . CP . openpilotLongitudinalControl :
if self . frame % 100 == 0 :
if self . frame % 100 == 0 :
can_sends . append ( [ 0x7D0 , 0 , b " \x02 \x3E \x80 \x00 \x00 \x00 \x00 \x00 " , 0 ] )
can_sends . append ( [ 0x7D0 , 0 , b " \x02 \x3E \x80 \x00 \x00 \x00 \x00 \x00 " , 0 ] )
can_sends . append ( create_lkas11 ( self . packer , self . frame , self . car_fingerprint , apply_steer , CC . latActive ,
can_sends . append ( hyundaican . create_lkas11 ( self . packer , self . frame , self . car_fingerprint , apply_steer , CC . latActive ,
CS . lkas11 , sys_warning , sys_state , CC . enabled ,
CS . lkas11 , sys_warning , sys_state , CC . enabled ,
hud_control . leftLaneVisible , hud_control . rightLaneVisible ,
hud_control . leftLaneVisible , hud_control . rightLaneVisible ,
left_lane_warning , right_lane_warning ) )
left_lane_warning , right_lane_warning ) )
if not self . CP . openpilotLongitudinalControl :
if not self . CP . openpilotLongitudinalControl :
if pcm_cancel_cmd :
if CC . cruiseControl . cancel :
can_sends . append ( create_clu11 ( self . packer , self . frame , CS . clu11 , Buttons . CANCEL ) )
can_sends . append ( hyundaican . create_clu11 ( self . packer , self . frame , CS . clu11 , Buttons . CANCEL ) )
elif CS . out . cruiseState . standstill :
elif CS . out . cruiseState . standstill :
# send resume at a max freq of 10Hz
# send resume at a max freq of 10Hz
if ( self . frame - self . last_resume_frame ) * DT_CTRL > 0.1 :
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
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends . extend ( [ create_clu11 ( self . packer , self . frame , CS . clu11 , Buttons . RES_ACCEL ) ] * 25 )
can_sends . extend ( [ hyundaican . create_clu11 ( self . packer , self . frame , CS . clu11 , Buttons . RES_ACCEL ) ] * 25 )
self . last_resume _frame = self . frame
self . last_button _frame = self . frame
if self . frame % 2 == 0 and self . CP . openpilotLongitudinalControl :
if self . frame % 2 == 0 and self . CP . openpilotLongitudinalControl :
accel = actuators . accel
accel = actuators . accel
@ -101,7 +117,7 @@ class CarController:
lead_visible = False
lead_visible = False
stopping = actuators . longControlState == LongCtrlState . stopping
stopping = actuators . longControlState == LongCtrlState . stopping
set_speed_in_units = hud_control . setSpeed * ( CV . MS_TO_MPH if CS . clu11 [ " CF_Clu_SPEED_UNIT " ] == 1 else CV . MS_TO_KPH )
set_speed_in_units = hud_control . setSpeed * ( CV . MS_TO_MPH if CS . clu11 [ " CF_Clu_SPEED_UNIT " ] == 1 else CV . MS_TO_KPH )
can_sends . extend ( create_acc_commands ( self . packer , CC . enabled , accel , jerk , int ( self . frame / 2 ) , lead_visible ,
can_sends . extend ( hyundaican . create_acc_commands ( self . packer , CC . enabled , accel , jerk , int ( self . frame / 2 ) , lead_visible ,
set_speed_in_units , stopping , CS . out . gasPressed ) )
set_speed_in_units , stopping , CS . out . gasPressed ) )
self . accel = accel
self . accel = accel
@ -110,15 +126,15 @@ class CarController:
CAR . IONIQ_EV_2020 , CAR . IONIQ_PHEV , CAR . KIA_CEED , CAR . KIA_SELTOS , CAR . KONA_EV ,
CAR . IONIQ_EV_2020 , CAR . IONIQ_PHEV , CAR . KIA_CEED , CAR . KIA_SELTOS , CAR . KONA_EV ,
CAR . ELANTRA_2021 , CAR . ELANTRA_HEV_2021 , CAR . SONATA_HYBRID , CAR . KONA_HEV , CAR . SANTA_FE_2022 ,
CAR . ELANTRA_2021 , CAR . ELANTRA_HEV_2021 , CAR . SONATA_HYBRID , CAR . KONA_HEV , CAR . SANTA_FE_2022 ,
CAR . KIA_K5_2021 , CAR . IONIQ_HEV_2022 , CAR . SANTA_FE_HEV_2022 , CAR . GENESIS_G70_2020 , CAR . SANTA_FE_PHEV_2022 ) :
CAR . KIA_K5_2021 , CAR . IONIQ_HEV_2022 , CAR . SANTA_FE_HEV_2022 , CAR . GENESIS_G70_2020 , CAR . SANTA_FE_PHEV_2022 ) :
can_sends . append ( create_lfahda_mfc ( self . packer , CC . enabled ) )
can_sends . append ( hyundaican . create_lfahda_mfc ( self . packer , CC . enabled ) )
# 5 Hz ACC options
# 5 Hz ACC options
if self . frame % 20 == 0 and self . CP . openpilotLongitudinalControl :
if self . frame % 20 == 0 and self . CP . openpilotLongitudinalControl :
can_sends . extend ( create_acc_opt ( self . packer ) )
can_sends . extend ( hyundaican . create_acc_opt ( self . packer ) )
# 2 Hz front radar options
# 2 Hz front radar options
if self . frame % 50 == 0 and self . CP . openpilotLongitudinalControl :
if self . frame % 50 == 0 and self . CP . openpilotLongitudinalControl :
can_sends . append ( create_frt_radar_opt ( self . packer ) )
can_sends . append ( hyundaican . create_frt_radar_opt ( self . packer ) )
new_actuators = actuators . copy ( )
new_actuators = actuators . copy ( )
new_actuators . steer = apply_steer / self . params . STEER_MAX
new_actuators . steer = apply_steer / self . params . STEER_MAX