def steerParams_torque(self, CS, actuators, path_plan ):
param = SteerLimitParams()
v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH
dst_steer = actuators.steer * param.STEER_MAX
abs_angle_steers = abs(actuators.steerAngle)
self.enable_time = self.timer1.sampleTime()
if self.enable_time < 50:
self.steer_torque_over_timer = 0
self.steer_torque_ratio = 1
return param, dst_steer
nMAX, nUP, nDN = self.atom_tune( v_ego_kph, self.model_speed )
param.STEER_MAX = min( param.STEER_MAX, nMAX)
param.STEER_DELTA_UP = min( param.STEER_DELTA_UP, nUP)
param.STEER_DELTA_DOWN = min( param.STEER_DELTA_DOWN, nDN )
sec_mval = 10.0 # 오파 => 운전자. (sec)
sec_pval = 3 # 운전자 => 오파 (sec)
# streer over check
if path_plan.laneChangeState != LaneChangeState.off:
self.steer_torque_over_timer = 0
elif CS.out.leftBlinker or CS.out.rightBlinker:
sec_mval = 0.5 # 오파 => 운전자.
sec_pval = 10 # 운전자 => 오파 (sec)
if v_ego_kph > 5 and CS.out.steeringPressed: #사용자 핸들 토크
if abs_angle_steers > 5 and CS.out.steeringTorque < -10: #-STEER_THRESHOLD: #right
if dst_steer < 0:
self.steer_torque_over_timer = 0
else:
#sec_mval = 1
self.steer_torque_over_timer = 50
elif abs_angle_steers > 5 and CS.out.steeringTorque > 10: #STEER_THRESHOLD: #left
if dst_steer > 0:
self.steer_torque_over_timer = 0
else:
#sec_mval = 1
self.steer_torque_over_timer = 50
else:
self.steer_torque_over_timer = 50
elif self.steer_torque_over_timer:
self.steer_torque_over_timer -= 1
ratio_pval = 1/(100*sec_pval)
ratio_mval = 1/(100*sec_mval)
if self.steer_torque_over_timer:
self.steer_torque_ratio -= ratio_mval
else:
self.steer_torque_ratio += ratio_pval
if self.steer_torque_ratio < 0:
self.steer_torque_ratio = 0
elif self.steer_torque_ratio > 1:
self.steer_torque_ratio = 1
return param, dst_steer
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.cp_oplongcontrol = CP.openpilotLongitudinalControl
self.packer = CANPacker(dbc_name)
self.accel_steady = 0
self.accel_lim_prev = 0.
self.accel_lim = 0.
self.steer_rate_limited = False
self.p = SteerLimitParams(CP)
self.usestockscc = True
self.lead_visible = False
self.lead_debounce = 0
self.gapsettingdance = 2
self.gapcount = 0
self.current_veh_speed = 0
self.lfainFingerprint = CP.lfaAvailable
self.vdiff = 0
self.resumebuttoncnt = 0
self.lastresumeframe = 0
self.fca11supcnt = self.fca11inc = self.fca11alivecnt = self.fca11cnt13 = self.scc11cnt = self.scc12cnt = 0
self.counter_init = False
self.fca11maxcnt = 0xD
self.radarDisableActivated = False
self.radarDisableResetTimer = 0
self.radarDisableOverlapTimer = 0
self.sendaccmode = not CP.radarDisablePossible
self.enabled = False
self.sm = messaging.SubMaster(['controlsState'])
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above_timer = 0
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
self.last_resume_frame = 0
self.p = SteerLimitParams(CP)
def steerParams_torque(self, CS, abs_angle_steers, path_plan):
param = SteerLimitParams()
v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH
self.enable_time = self.timer1.sampleTime()
if self.enable_time < 50:
self.steer_torque_over_timer = 0
self.steer_torque_ratio = 1
return param
nMAX, nUP, nDN = self.atom_tune(CS.out.vEgo, self.model_speed)
param.STEER_MAX = min(param.STEER_MAX, nMAX)
param.STEER_DELTA_UP = min(param.STEER_DELTA_UP, nUP)
param.STEER_DELTA_DOWN = min(param.STEER_DELTA_DOWN, nDN)
sec_pval = 0.5 # 0.5 sec 운전자 => 오파 (sec)
sec_mval = 30.0 # 오파 => 운전자. (sec)
# streer over check
if path_plan.laneChangeState != LaneChangeState.off:
self.steer_torque_over_timer = 0
elif CS.out.leftBlinker or CS.out.rightBlinker:
sec_mval = 0.2 # 오파 => 운전자.
sec_pval = 10
if v_ego_kph > 5 and CS.out.steeringPressed: #사용자 핸들 토크
self.steer_torque_over_timer = 50
elif self.steer_torque_over_timer:
self.steer_torque_over_timer -= 1
ratio_pval = 1 / (100 * sec_pval)
ratio_mval = 1 / (100 * sec_mval)
if self.steer_torque_over_timer:
self.steer_torque_ratio -= ratio_mval
else:
self.steer_torque_ratio += ratio_pval
if self.steer_torque_ratio < 0:
self.steer_torque_ratio = 0
elif self.steer_torque_ratio > 1:
self.steer_torque_ratio = 1
return param
def __init__(self, dbc_name, CP, VM):
self.p = SteerLimitParams(CP)
self.packer = CANPacker(dbc_name)
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.steer_rate_limited = False
self.last_resume_frame = 0
# dp
self.last_blinker_on = False
self.blinker_end_frame = 0.
self.dp_hkg_smart_mdps = Params().get('dp_hkg_smart_mdps') == b'1'
def __init__(self, dbc_name, CP, VM):
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.accel_steady = 0
self.apply_steer_last = 0
self.steer_rate_limited = False
self.lkas11_cnt = 0
self.scc12_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.turning_signal_timer = 0
self.lkas_button_on = True
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
if CP.spasEnabled:
self.en_cnt = 0
self.apply_steer_ang = 0.0
self.en_spas = 3
self.mdps11_stat_last = 0
self.spas_always = False
self.p = SteerLimitParams(CP)
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.p = SteerLimitParams(CP)
self.packer = CANPacker(dbc_name)
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.steer_rate_limited = False
self.last_resume_frame = 0
self.last_lead_distance = 0
self.resume_cnt = 0
self.lkas11_cnt = 0
self.nBlinker = 0
self.lane_change_torque_lower = 0
self.steer_torque_over_timer = 0
self.steer_torque_ratio = 1
self.steer_torque_ratio_dir = 1
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.timer1 = tm.CTime1000("time")
self.model_speed = 0
self.model_sum = 0
# hud
self.hud_timer_left = 0
self.hud_timer_right = 0
self.hud_sys_state = 0
self.command_cnt = 0
self.command_load = 0
self.params = Params()
self.SC = SpdctrlSlow()
self.traceCC = trace1.Loger("CarController")
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, sm, LaC):
path_plan = sm['pathPlan']
# *** compute control surfaces ***
v_ego_kph = CS.v_ego * CV.MS_TO_KPH
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = self.accel_hysteresis(
apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
abs_angle_steers = abs(
actuators.steerAngle) # abs(CS.angle_steers) #
param = SteerLimitParams()
if path_plan.laneChangeState != LaneChangeState.off:
#param.STEER_MAX *= 0.99
param.STEER_DELTA_UP = 1
param.STEER_DELTA_DOWN = 5
#v_curvature
elif LaC.v_curvature < 200:
self.timer_curvature = 300
elif abs_angle_steers < 1 and self.timer_curvature <= 0:
xp = [0.5, 1]
fp = [240, 255]
param.STEER_MAX = interp(abs_angle_steers, xp, fp)
if abs_angle_steers < 1 or v_ego_kph < 5:
param.STEER_DELTA_UP = 2
param.STEER_DELTA_DOWN = 4
if self.timer_curvature:
self.timer_curvature -= 1
### Steering Torque
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.steer_torque_driver,
param)
self.steer_rate_limited = new_steer != apply_steer
if abs(CS.steer_torque_driver) > self.steer_torque_over_max: #200:
self.steer_torque_over_timer += 1
if self.steer_torque_over_timer > 5:
self.steer_torque_over = True
self.steer_torque_over_timer = 100
elif self.steer_torque_over_timer:
self.steer_torque_over_timer -= 1
else:
self.steer_torque_over = False
### LKAS button to temporarily disable steering
#if not CS.lkas_error:
# if self.lkas_button != CS.lkas_button_on:
# self.lkas_button = CS.lkas_button_on
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
if self.car_fingerprint == CAR.GENESIS:
lkas_active = enabled and abs(CS.angle_steers) < 90.
else:
lkas_active = enabled and abs(CS.angle_steers) < 100.
# fix for Genesis hard fault at low speed
if v_ego_kph < 60 and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus:
lkas_active = 0
low_speed = self.low_speed_car
#if not self.lkas_button:
# low_speed = False
#elif not CS.cruiseState.enabled:
# low_speed = False
if CS.stopped:
low_speed = False
elif CS.v_ego > (CS.CP.minSteerSpeed + 0.7):
low_speed = False
elif CS.v_ego < (CS.CP.minSteerSpeed + 0.2):
low_speed = True
if self.low_speed_car != low_speed:
self.low_speed_car = low_speed
# streer over check
if enabled and abs(CS.angle_steers) > 100. or CS.steer_error:
self.streer_angle_over = True
self.steer_timer = 250
elif abs(CS.angle_steers) < 7.5 or not self.steer_timer:
self.streer_angle_over = False
elif self.steer_timer:
self.steer_timer -= 1
# Disable steering while turning blinker on and speed below 60 kph
if CS.left_blinker_on or CS.right_blinker_on:
self.steer_torque_over = False
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
elif CS.left_blinker_flash or CS.right_blinker_flash:
self.steer_torque_over = False
self.turning_signal_timer = 100
# turning indicator alert logic
if self.lane_change_enabled:
self.turning_indicator = self.turning_signal_timer and CS.v_ego < LaneChangeParms.LANE_CHANGE_SPEED_MIN
else:
self.turning_indicator = self.turning_signal_timer
if self.turning_signal_timer:
self.turning_signal_timer -= 1
if left_line:
self.hud_timer_left = 100
elif self.hud_timer_left:
self.hud_timer_left -= 1
if right_line:
self.hud_timer_right = 100
elif self.hud_timer_right:
self.hud_timer_right -= 1
if path_plan.laneChangeState != LaneChangeState.off:
if LaC.v_curvature > 200:
self.lkas_active_timer1 = 300
apply_steer = self.limit_ctrl(apply_steer, 100, 0)
elif not self.hud_timer_left and not self.hud_timer_right:
self.lkas_active_timer1 = 200 # apply_steer = 70
elif path_plan.laneChangeState != LaneChangeState.off:
self.steer_torque_over = False
# disable lkas
if not CS.main_on:
lkas_active = 0
self.turning_signal_timer = 0
self.turning_indicator = False
self.steer_torque_over = False
elif CS.stopped:
lkas_active = 0
elif self.steer_torque_over:
lkas_active = 0
if self.streer_angle_over:
lkas_active = 0
elif self.turning_indicator:
lkas_active = 0
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
steer_limit = param.STEER_MAX #param.STEER_MAX
if not lkas_active:
self.lkas_active_timer1 = 200
elif self.lkas_active_timer1 < 400:
self.lkas_active_timer1 += 1
ratio_steer = self.lkas_active_timer1 / 400
if ratio_steer < 1:
steer_limit = ratio_steer * param.STEER_MAX
apply_steer = self.limit_ctrl(apply_steer, steer_limit, 0)
dRel, yRel, vRel = self.SC.get_lead(sm, CS)
vRel = int(vRel * 3.6 + 0.5)
lead_objspd = CS.lead_objspd
str_log1 = 'CV={:03.0f}/{:06.3f} TQ=V:{:04.0f}/S:{:04.0f}'.format(
LaC.v_curvature, LaC.model_sum, apply_steer,
CS.steer_torque_driver)
str_log2 = 'D={:05.1f} V={:03.0f} S_LIM={:03.0f} S_MAX={:03.0f}'.format(
dRel, vRel, steer_limit, param.STEER_MAX)
trace1.printf('{} {}'.format(str_log1, str_log2))
self.apply_accel_last = apply_accel
self.apply_steer_last = apply_steer
hud_alert, lane_visible = self.process_hud_alert(
lkas_active, self.lkas_button, visual_alert, self.hud_timer_left,
self.hud_timer_right, CS)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 60
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
self.clu11_cnt = frame % 0x10
self.mdps12_cnt = frame % 0x100
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
0,
apply_steer,
steer_req,
self.lkas11_cnt,
lkas_active,
CS.lkas11,
hud_alert,
lane_visible,
keep_stock=True))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas12 bus 1 if mdps or scc is on bus 1
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
1,
apply_steer,
steer_req,
self.lkas11_cnt,
lkas_active,
CS.lkas11,
hud_alert,
lane_visible,
keep_stock=True))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, CS.mdps_bus, CS.clu11, Buttons.NONE,
enabled_speed, self.clu11_cnt))
if pcm_cancel_cmd and self.longcontrol:
can_sends.append(
create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL,
clu11_speed, self.clu11_cnt))
else: # send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(
create_mdps12(self.packer, self.car_fingerprint,
self.mdps12_cnt, CS.mdps12))
# AVM
#if CS.mdps_bus:
#if not CS.cp_AVM.can_valid:
#can_sends.append(create_AVM(self.packer, self.car_fingerprint, CS.avm, CS ))
if CS.stopped:
#self.model_speed = 300
# run only first time when the car stopped
if self.last_lead_distance == 0:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed,
self.resume_cnt))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif CS.driverOverride == 2 or not CS.pcm_acc_status or CS.clu_CruiseSwState == 1 or CS.clu_CruiseSwState == 2:
#self.model_speed = 300
self.resume_cnt = 0
self.sc_btn_type = Buttons.NONE
self.sc_wait_timer2 = 10
self.sc_active_timer2 = 0
elif self.sc_wait_timer2:
self.sc_wait_timer2 -= 1
elif self.speed_control_enabled:
#acc_mode, clu_speed = self.long_speed_cntrl( v_ego_kph, CS, actuators )
btn_type, clu_speed = self.SC.update(
v_ego_kph, CS, sm, actuators, dRel, yRel, vRel,
LaC.v_curvature) # speed controller spdcontroller.py
if CS.clu_Vanz < 5:
self.sc_btn_type = Buttons.NONE
elif self.sc_btn_type != Buttons.NONE:
pass
elif btn_type != Buttons.NONE:
self.resume_cnt = 0
self.sc_active_timer2 = 0
self.sc_btn_type = btn_type
self.sc_clu_speed = clu_speed
if self.sc_btn_type != Buttons.NONE:
self.sc_active_timer2 += 1
if self.sc_active_timer2 > 10:
self.sc_wait_timer2 = 5
self.resume_cnt = 0
self.sc_active_timer2 = 0
self.sc_btn_type = Buttons.NONE
else:
#self.traceCC.add( 'sc_btn_type={} clu_speed={} set={:.0f} vanz={:.0f}'.format( self.sc_btn_type, self.sc_clu_speed, CS.VSetDis, clu11_speed ) )
can_sends.append(
create_clu11(self.packer, CS.scc_bus, CS.clu11,
self.sc_btn_type, self.sc_clu_speed,
self.resume_cnt))
self.resume_cnt += 1
self.lkas11_cnt += 1
return can_sends
def steerParams_torque(self, CS, abs_angle_steers, path_plan, CC):
param = SteerLimitParams()
v_ego_kph = CS.out.vEgo * CV.MS_TO_KPH
self.cV_tune(CS.out.vEgo, self.model_speed)
param.STEER_MAX = min(param.STEER_MAX, self.MAX)
param.STEER_DELTA_UP = min(param.STEER_DELTA_UP, self.UP)
param.STEER_DELTA_DOWN = min(param.STEER_DELTA_DOWN, self.DN)
# streer over check
if v_ego_kph > 5 and abs(CS.out.steeringTorque) > 180: #사용자 핸들 토크
self.steer_torque_over_timer = 1
else:
self.steer_torque_over_timer = 0
if CS.out.leftBlinker or CS.out.rightBlinker:
self.nBlinker += 1
elif self.nBlinker:
self.nBlinker = 0
# 차선이 없고 앞차량이 없으면.
steer_angle_lower = self.dRel > 20 and (
not CC.hudControl.leftLaneVisible
and not CC.hudControl.rightLaneVisible)
if v_ego_kph < 1:
self.steer_torque_over_timer = 0
self.steer_torque_ratio_dir = 1
elif path_plan.laneChangeState != LaneChangeState.off:
self.steer_torque_ratio_dir = 1
self.steer_torque_over_timer = 0
self.nBlinker = 0
elif self.steer_torque_over_timer: #or CS.out.steerWarning:
self.steer_torque_ratio_dir = -1
elif steer_angle_lower:
param.STEER_MAX *= 0.5
param.STEER_DELTA_UP = 1
param.STEER_DELTA_DOWN = 2
self.steer_torque_ratio_dir = 1
else:
self.steer_torque_ratio_dir = 1
lane_change_torque_lower = 0
if self.nBlinker > 10 and v_ego_kph > 1:
lane_change_torque_lower = int(
CS.out.leftBlinker) + int(CS.out.rightBlinker) * 2
if CS.out.steeringPressed and self.param_OpkrWhoisDriver:
self.steer_torque_ratio = 0.05
self.lane_change_torque_lower = lane_change_torque_lower
# smoth torque enable or disable
ratio_pval = 0.001 # 10 sec
ratio_mval = 0.001 # 10 sec
if self.param_OpkrWhoisDriver == 1: # 민감
ratio_pval = 0.005 # 2 sec
ratio_mval = 0.01 # 1 sec
else: # 보통.
ratio_pval = 0.002 # 5 sec
ratio_mval = 0.005 # 2 sec
if self.param_OpkrWhoisDriver == 0:
self.steer_torque_ratio = 1
elif self.steer_torque_ratio_dir >= 1:
if self.steer_torque_ratio < 1:
self.steer_torque_ratio += ratio_pval
elif self.steer_torque_ratio_dir <= -1:
if self.steer_torque_ratio > 0:
self.steer_torque_ratio -= ratio_mval
if self.steer_torque_ratio < 0:
self.steer_torque_ratio = 0
elif self.steer_torque_ratio > 1:
self.steer_torque_ratio = 1
return param
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
else:
self.model_speed = self.model_sum = 0
# Steering Torque
if self.param_OpkrEnableLearner:
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
else:
path_plan = sm['pathPlan']
self.cV_tune(CS.out.vEgo, self.model_speed)
param = SteerLimitParams()
param.STEER_MAX = min(param.STEER_MAX, self.MAX)
param.STEER_DELTA_UP = min(param.STEER_DELTA_UP, self.UP)
param.STEER_DELTA_DOWN = min(param.STEER_DELTA_DOWN, self.DN)
new_steer = actuators.steer * param.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, param)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 90.
if ((CS.out.leftBlinker and not CS.out.rightBlinker) or
(CS.out.rightBlinker
and not CS.out.leftBlinker)) and CS.out.vEgo < 60 * CV.KPH_TO_MS:
self.lanechange_manual_timer = 10
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 10
if abs(CS.out.steeringTorque
) > self.driver_steering_torque_above and CS.out.vEgo < 60:
self.driver_steering_torque_above_timer = 30
if self.lanechange_manual_timer or self.driver_steering_torque_above_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if self.driver_steering_torque_above_timer > 0:
self.driver_steering_torque_above_timer -= 1
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, CC)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, CC, enabled, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt,
self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled,
1))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
#if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if CS.mdps_bus:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = '곡률={:05.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format(
self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque)
if self.param_OpkrEnableLearner:
str_log2 = '프레임율={:03.0f} STMAX={:03.0f}'.format(
self.timer1.sampleTime(),
SteerLimitParams.STEER_MAX,
)
else:
str_log2 = '프레임율={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f} SR={:05.2f}'.format(
self.timer1.sampleTime(), self.MAX, self.UP, self.DN,
path_plan.steerRatio)
trace1.printf('{} {}'.format(str_log1, str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None and (
CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2
or CS.out.cruiseState.modeSel == 3)
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "자동RES"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
if CS.out.cruiseState.modeSel == 3:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} AUTORES=(VS:{:03.0f}/CN:{:01.0f}/RD:{:03.0f}/BK:{})'.format(
self.steer_mode, self.mdps_status, self.lkas_switch,
CS.VSetDis, self.res_cnt, self.res_delay,
CS.out.brakeLights)
else:
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch)
trace1.printf2('{}'.format(str_log2))
#print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) )
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
elif CS.out.cruiseState.standstill and not self.car_fingerprint == CAR.NIRO_EV:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif CS.out.cruiseState.standstill and self.car_fingerprint == CAR.NIRO_EV:
if CS.lead_distance > 3.7 and (
frame - self.last_resume_frame
) * DT_CTRL > 0.2 and self.param_OpkrAutoResume:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.last_resume_frame = frame
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, self.SC.btn_type,
self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if CS.out.cruiseState.modeSel == 3:
if CS.out.brakeLights and CS.VSetDis > 30:
self.res_cnt = 0
self.res_delay = 50
elif self.res_delay:
self.res_delay -= 1
elif not self.res_delay and self.res_cnt < 6 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS:
if self.res_cnt < 1:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.res_cnt += 1
else:
self.res_cnt = 7
self.res_delay = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
