def update(self, c, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = c.enabled
actuators = c.actuators
pcm_cancel_cmd = c.cruiseControl.cancel
path_plan = sm['pathPlan']
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
param, dst_steer = self.steerParams_torque(CS, c.actuators, path_plan)
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
apply_steer_limit = param.STEER_MAX
if self.steer_torque_ratio < 1:
apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX)
apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0)
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and CS.main_on and CS.out.cruiseState.enabled and abs(
CS.out.steeringAngle) < 180. #and self.lkas_button
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, self.hud_sys_state = self.process_hud_alert(
lkas_active, c)
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,
self.hud_sys_state, c))
# send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = 'torg:{:5.0f}/{:5.0f}/{:5.0f} CV={:5.1f}'.format(
apply_steer, new_steer, dst_steer, self.model_speed)
str_log2 = 'limit={:.0f} tm={:.1f} '.format(apply_steer_limit,
self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, str_log2))
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
str_log2 = 'LKAS={:.0f} steer={:5.0f} '.format(
CS.lkas_button_on, CS.out.steeringTorque)
trace1.printf2('{}'.format(str_log2))
if pcm_cancel_cmd and self.CP.longcontrolEnabled:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# 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, self.resume_cnt, CS.clu11,
Buttons.RES_ACCEL))
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 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.clu11,
self.SC.btn_type, self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
str1 = 'run={} cruise_set_mode={} kph={:.1f}/{:.1f} DO={:.0f}/{:.0f} '.format(
is_sc_run, self.SC.cruise_set_mode,
self.SC.cruise_set_speed_kph, CS.VSetDis, CS.driverOverride,
CS.cruise_buttons)
str2 = 'btn_type={:.0f} speed={:.1f} cnt={:.0f}'.format(
self.SC.btn_type, self.SC.sc_clu_speed, self.resume_cnt)
str_log = str1 + str2
self.traceCC.add(str_log)
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.SONATA, CAR.PALISADE
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
# counter inc
self.lkas11_cnt += 1
return can_sends
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 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
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
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 180. #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) > 409: #360: #180
self.driver_steering_torque_above_timer = 100
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 = '곡률={:04.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format(
self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque)
str_log2 = '프레임율={:03.0f}'.format(self.timer1.sampleTime())
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
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 = "-"
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 < 0 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS:
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
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
# Steering Torque
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
# 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 <= 59 * 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) > 200:
self.driver_steering_torque_above_timer = 15
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}'.format(self.model_speed)
str_log2 = '프레임율={:03.0f} TPMS=FL:{:04.1f}/FR:{:04.1f}/RL:{:04.1f}/RR:{:04.1f}'.format(
self.timer1.sampleTime(), CS.tpmsPressureFl, CS.tpmsPressureFr,
CS.tpmsPressureRl, CS.tpmsPressureRr)
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 = "정체구간"
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} CG:{:1.0f}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch,
CS.cruiseGapSet)
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:
# 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 self.cruise_gap_prev == 0:
self.cruise_gap_prev = CS.cruiseGapSet
self.cruise_gap_set_init = 1
elif CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 100:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
self.cruise_gap = 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 self.dRel > 17 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1 and CS.out.cruiseState.modeSel != 3:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 50:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif self.cruise_gap_prev == CS.cruiseGapSet:
self.cruise_gap_set_init = 0
self.cruise_gap_prev = 0
if CS.out.cruiseState.modeSel == 3 and CS.acc_active:
if 20 > self.dRel > 18 and self.vRel < 0 and CS.cruiseGapSet != 4.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 16 > self.dRel > 14 and self.vRel < 0 and CS.cruiseGapSet != 3.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 12 > self.dRel > 10 and self.vRel < 0 and CS.cruiseGapSet != 2.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 9 > self.dRel > 7 and self.vRel < 0 and CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 15 > self.dRel > 4 and self.vRel > 0 and CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif 25 > self.dRel > 18 and self.vRel >= 0 and CS.cruiseGapSet != 2.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 30:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
# 20 Hz LFA MFA message
#if frame % 5 == 0 and self.car_fingerprint in [CAR.IONIQ]:
# can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_lane, right_lane, left_lane_depart,
right_lane_depart):
# Steering Torque
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
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled #and abs(CS.out.steeringAngle) < 90.
# fix for Genesis hard fault at low speed
#if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
#lkas_active = False
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, left_lane_warning, right_lane_warning =\
process_hud_alert(enabled, self.car_fingerprint, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 60
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"]
self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10
can_sends = []
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc 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, enabled,
left_lane, right_lane, left_lane_warning,
right_lane_warning, 1))
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 = 'torg:{:3.0f}'.format(apply_steer)
str_log2 = 'new_steer={:.0f} tm={:.1f} '.format(
new_steer, self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, 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.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
if self.last_lead_distance == 0:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
# SCC won't resume anyway when the lead distace is less than 3.7m
# send resume at a max freq of 5Hz
#if CS.lead_distance > 3.7 and (frame - self.last_resume_frame)*DT_CTRL > 0.2:
if CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) * DT_CTRL > 0.2:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.last_resume_frame = frame
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.SONATA, CAR.PALISADE, CAR.IONIQ
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
return can_sends
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. #TenesiDel -> and self.lkas_button_on
# fix for Genesis hard fault at low speed 테네시 추가
if CS.out.vEgo < 60 * CV.KPH_TO_MS and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus:
lkas_active = False
# 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
# Disable steering while turning blinker on and speed below 60 kph #201011 상시조향 작업 작동성공
if CS.out.leftBlinker or CS.out.rightBlinker:
if self.car_fingerprint not in [CAR.K5, CAR.K5_HEV]: # 테네시 추가 OPTIMA -> K5
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
elif CS.left_blinker_flash or CS.right_blinker_flash: # Optima has blinker flash signal only
self.turning_signal_timer = 100
if self.turning_signal_timer and CS.out.vEgo > 70 * CV.KPH_TO_MS: # TenesiADD Blinker tune 시속60미만에서는 상시조향
lkas_active = 0
if self.turning_signal_timer: # TenesiADD
self.turning_signal_timer -= 1 #201011 상시조향 작업 작동성공
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
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
path_plan = sm['pathPlan']
abs_angle_steers = abs(actuators.steerAngle)
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
param = self.steerParams_torque(CS, abs_angle_steers, path_plan, CC)
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
apply_steer_limit = param.STEER_MAX
if self.steer_torque_ratio < 1:
apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX)
apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0)
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(
CS.out.steeringAngle) < 180. #and self.lkas_button
# fix for Genesis hard fault at low speed
#if CS.out.vEgo < 16.666667 and self.car_fingerprint == CAR.GENESIS:
# lkas_active = 0
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 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
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
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if pcm_cancel_cmd and self.longcontrol:
# can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
#else: # send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
#str_log1 = 'CV={:5.1f}/{:5.3f} torg:{:5.0f}'.format( self.model_speed, self.model_sum, apply_steer )
str_log1 = 'CV={:5.1f} torg:{:6.1f}'.format(self.model_speed,
apply_steer)
#str_log2 = 'limit={:.0f} tm={:.1f} '.format( apply_steer_limit, self.timer1.sampleTime() )
str_log2 = ' limit={:6.1f}/tm={:3.1f} MAX={:5.1f} UP/DN={:3.1f}/{:3.1f} '.format(
apply_steer_limit, self.timer1.sampleTime(), self.MAX, self.UP,
self.DN)
trace1.printf('{} {}'.format(str_log1, str_log2))
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
str_log2 = 'U={:.0f} LK={:.0f} dir={} steer={:5.0f} '.format(
CS.Mdps_ToiUnavail, CS.lkas_button_on,
self.steer_torque_ratio_dir, CS.out.steeringTorque)
trace1.printf2('{}'.format(str_log2))
if pcm_cancel_cmd and self.CP.longcontrolEnabled:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# 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 < CS.lead_distance > 4.8 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 10 msgs
if self.resume_cnt > 10:
self.last_resume_frame = frame
self.resume_cnt = 0
self.clu11_cnt = 0
# 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
str1 = 'run={} cruise_set_mode={} kph={:.1f}/{:.1f} DO={:.0f}/{:.0f} '.format(
is_sc_run, self.SC.cruise_set_mode,
self.SC.cruise_set_speed_kph, CS.VSetDis, CS.driverOverride,
CS.cruise_buttons)
str2 = 'btn_type={:.0f} speed={:.1f} cnt={:.0f}'.format(
self.SC.btn_type, self.SC.sc_clu_speed, self.resume_cnt)
str_log = str1 + str2
self.traceCC.add(str_log)
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.PALISADE, CAR.SELTOS
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
# counter inc
self.lkas11_cnt += 1
return can_sends
def update(self, c, CS, frame, sm):
enabled = c.enabled
actuators = c.actuators
pcm_cancel_cmd = c.cruiseControl.cancel
abs_angle_steers = abs(actuators.steerAngle)
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
# Steering Torque
path_plan = sm['pathPlan']
param = self.steerParams_torque(CS, abs_angle_steers, path_plan)
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
apply_steer_limit = param.STEER_MAX
if self.steer_torque_ratio < 1:
apply_steer_limit = int(self.steer_torque_ratio * param.STEER_MAX)
apply_steer = self.limit_ctrl(apply_steer, apply_steer_limit, 0)
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 120.
# fix for Genesis hard fault at low speed
if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
lkas_active = False
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, c)
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"]
self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10
can_sends = []
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, c))
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = 'torg:{:>4.0f}/{:>4.0f} CV:{:>5.0f}'.format(
apply_steer, new_steer, self.model_speed)
str_log2 = 'max={:>4.0f} tm={:>5.1f} '.format(apply_steer_limit,
self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, str_log2))
lfa_usm = CS.lfahda["LFA_USM"]
lfa_warn = CS.lfahda["LFA_SysWarning"]
lfa_active = CS.lfahda["ACTIVE2"]
hda_usm = CS.lfahda["HDA_USM"]
hda_active = CS.lfahda["ACTIVE"]
str_log1 = 'hda={:.0f},{:.0f}'.format(hda_usm, hda_active)
str_log2 = 'lfa={:.0f},{:.0f},{:.0f}'.format(lfa_usm, lfa_warn,
lfa_active)
trace1.printf2('{} {}'.format(str_log1, str_log2))
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# 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, self.resume_cnt, CS.clu11,
Buttons.RES_ACCEL))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.SONATA, CAR.PALISADE, CAR.IONIQ
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
return can_sends