def update(self, c, enabled, CS, frame, CC, actuators, pcm_cancel_cmd,
visual_alert, left_lane, right_lane, left_lane_depart,
right_lane_depart, set_speed, lead_visible, controls):
# Steering Torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
CarControllerParams)
# disable when temp fault is active, or below LKA minimum speed
lkas_active = c.active and not CS.out.steerWarning and abs(
CS.out.steeringAngleDeg) < CS.CP.maxSteeringAngleDeg
# Disable steering while turning blinker on and speed below 60 kph
if CS.out.leftBlinker or CS.out.rightBlinker:
self.turning_signal_timer = 0.5 / DT_CTRL # Disable for 0.5 Seconds after blinker turned off
if self.turning_indicator_alert: # set and clear by interface
lkas_active = 0
if self.turning_signal_timer > 0:
self.turning_signal_timer -= 1
if not lkas_active:
apply_steer = 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)
if self.haptic_feedback_speed_camera:
if self.prev_active_cam != self.scc_smoother.active_cam:
self.prev_active_cam = self.scc_smoother.active_cam
if self.scc_smoother.active_cam:
self.active_cam_timer = int(1.5 / DT_CTRL)
if self.active_cam_timer > 0:
self.active_cam_timer -= 1
left_lane_warning = right_lane_warning = 1
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
if not (min_set_speed < set_speed < 255 * CV.KPH_TO_MS):
set_speed = min_set_speed
set_speed *= CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"]
# TODO: fix this
# self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
# self.scc_update_frame = frame
# check if SCC is alive
# if frame % 7 == 0:
# if CS.scc11["AliveCounterACC"] == self.prev_scc_cnt:
# if frame - self.scc_update_frame > 20 and self.scc_live:
# self.scc_live = False
# else:
# self.scc_live = True
# self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
# self.scc_update_frame = frame
self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10
can_sends = []
can_sends.append(
create_lkas11(self.packer, frame, self.car_fingerprint,
apply_steer, lkas_active, CS.lkas11, sys_warning,
sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 0,
self.ldws_opt))
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, frame, self.car_fingerprint,
apply_steer, lkas_active, CS.lkas11, sys_warning,
sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 1,
self.ldws_opt))
if frame % 2 and 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))
if pcm_cancel_cmd and (self.longcontrol and not self.mad_mode_enabled):
can_sends.append(
create_clu11(self.packer, CS.scc_bus, CS.clu11, Buttons.CANCEL,
clu11_speed))
else:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
# fix auto resume - by neokii
if CS.out.cruiseState.standstill and not CS.out.gasPressed:
if self.last_lead_distance == 0:
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
self.resume_wait_timer = 0
# scc smoother
elif self.scc_smoother.is_active(frame):
pass
elif self.resume_wait_timer > 0:
self.resume_wait_timer -= 1
elif abs(CS.lead_distance - self.last_lead_distance) > 0.1:
can_sends.append(
create_clu11(self.packer, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
if self.resume_cnt >= randint(6, 8):
self.resume_cnt = 0
self.resume_wait_timer = randint(30, 36)
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
# scc smoother
self.scc_smoother.update(enabled, can_sends, self.packer, CC, CS,
frame, controls)
# send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
if self.longcontrol and CS.cruiseState_enabled and (CS.scc_bus or
not self.scc_live):
if frame % 2 == 0:
stopping = controls.LoC.long_control_state == LongCtrlState.stopping
apply_accel = clip(actuators.accel if c.active else 0,
CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
apply_accel = self.scc_smoother.get_apply_accel(
CS, controls.sm, apply_accel, stopping)
self.accel = apply_accel
controls.apply_accel = apply_accel
aReqValue = CS.scc12["aReqValue"]
controls.aReqValue = aReqValue
if aReqValue < controls.aReqValueMin:
controls.aReqValueMin = controls.aReqValue
if aReqValue > controls.aReqValueMax:
controls.aReqValueMax = controls.aReqValue
if self.stock_navi_decel_enabled:
controls.sccStockCamAct = CS.scc11["Navi_SCC_Camera_Act"]
controls.sccStockCamStatus = CS.scc11[
"Navi_SCC_Camera_Status"]
apply_accel, stock_cam = self.scc_smoother.get_stock_cam_accel(
apply_accel, aReqValue, CS.scc11)
else:
controls.sccStockCamAct = 0
controls.sccStockCamStatus = 0
stock_cam = False
if self.scc12_cnt < 0:
self.scc12_cnt = CS.scc12[
"CR_VSM_Alive"] if not CS.no_radar else 0
self.scc12_cnt += 1
self.scc12_cnt %= 0xF
can_sends.append(
create_scc12(self.packer, apply_accel, enabled,
self.scc12_cnt, self.scc_live, CS.scc12,
CS.out.gasPressed, CS.out.brakePressed,
CS.out.cruiseState.standstill,
self.car_fingerprint))
can_sends.append(
create_scc11(self.packer, frame, enabled, set_speed,
lead_visible, self.scc_live, CS.scc11,
self.scc_smoother.active_cam, stock_cam))
if frame % 20 == 0 and CS.has_scc13:
can_sends.append(create_scc13(self.packer, CS.scc13))
if CS.has_scc14:
acc_standstill = stopping if CS.out.vEgo < 2. else False
lead = self.scc_smoother.get_lead(controls.sm)
if lead is not None:
d = lead.dRel
obj_gap = 1 if d < 25 else 2 if d < 40 else 3 if d < 60 else 4 if d < 80 else 5
else:
obj_gap = 0
can_sends.append(
create_scc14(self.packer, enabled, CS.out.vEgo,
acc_standstill, apply_accel,
CS.out.gasPressed, obj_gap, CS.scc14))
else:
self.scc12_cnt = -1
# 20 Hz LFA MFA message
if frame % 5 == 0:
activated_hda = road_speed_limiter_get_active()
# activated_hda: 0 - off, 1 - main road, 2 - highway
if self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(
create_lfahda_mfc(self.packer, enabled, activated_hda))
elif CS.has_lfa_hda:
can_sends.append(
create_hda_mfc(self.packer, activated_hda, CS, left_lane,
right_lane))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
new_actuators.accel = self.accel
return new_actuators, can_sends
def update(self, enabled, can_sends, packer, CC, CS, frame, controls):
# mph or kph
clu11_speed = CS.clu11["CF_Clu_Vanz"]
road_limit_speed, left_dist, max_speed_log = self.cal_max_speed(
frame, CC, CS, controls.sm, clu11_speed, controls)
CC.sccSmoother.roadLimitSpeedActive = road_speed_limiter_get_active()
CC.sccSmoother.roadLimitSpeed = road_limit_speed
CC.sccSmoother.roadLimitSpeedLeftDist = left_dist
# kph
controls.applyMaxSpeed = float(
clip(CS.cruiseState_speed * CV.MS_TO_KPH, MIN_SET_SPEED_KPH,
self.max_speed_clu * self.speed_conv_to_ms * CV.MS_TO_KPH))
CC.sccSmoother.longControl = self.longcontrol
CC.sccSmoother.applyMaxSpeed = controls.applyMaxSpeed
CC.sccSmoother.cruiseMaxSpeed = controls.v_cruise_kph
CC.sccSmoother.autoTrGap = AUTO_TR_CRUISE_GAP
ascc_enabled = CS.acc_mode and enabled and CS.cruiseState_enabled \
and 1 < CS.cruiseState_speed < 255 and not CS.brake_pressed
if not self.longcontrol:
if not ascc_enabled or CS.standstill or CS.cruise_buttons != Buttons.NONE:
self.reset()
self.wait_timer = max(ALIVE_COUNT) + max(WAIT_COUNT)
return
if not ascc_enabled:
self.reset()
self.cal_target_speed(CS, clu11_speed, controls)
CC.sccSmoother.logMessage = max_speed_log
if self.wait_timer > 0:
self.wait_timer -= 1
elif ascc_enabled and not CS.out.cruiseState.standstill:
if self.alive_timer == 0:
self.btn = self.get_button(CS.cruiseState_speed *
self.speed_conv_to_clu)
self.alive_count = SccSmoother.get_alive_count()
if self.btn != Buttons.NONE:
can_sends.append(
SccSmoother.create_clu11(packer, self.alive_timer,
CS.scc_bus, CS.clu11, self.btn))
if self.alive_timer == 0:
self.started_frame = frame
self.alive_timer += 1
if self.alive_timer >= self.alive_count:
self.alive_timer = 0
self.wait_timer = SccSmoother.get_wait_count()
self.btn = Buttons.NONE
else:
if self.longcontrol and self.target_speed >= self.min_set_speed_clu:
self.target_speed = 0.
else:
if self.longcontrol:
self.target_speed = 0.
def update(self, CC, CS, controls):
if self.CP.carFingerprint in HDA2_CAR:
return self.update_hda2(CC, CS)
actuators = CC.actuators
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
# Steering Torque
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
self.params)
# disable when temp fault is active, or below LKA minimum speed
lkas_active = CC.latActive
# Disable steering while turning blinker on and speed below 60 kph
if CS.out.leftBlinker or CS.out.rightBlinker:
self.turning_signal_timer = 0.5 / DT_CTRL # Disable for 0.5 Seconds after blinker turned off
if self.turning_indicator_alert: # set and clear by interface
lkas_active = 0
if self.turning_signal_timer > 0:
self.turning_signal_timer -= 1
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(
CC.enabled, self.CP.carFingerprint, hud_control)
if self.haptic_feedback_speed_camera:
if self.prev_active_cam != self.scc_smoother.active_cam:
self.prev_active_cam = self.scc_smoother.active_cam
if self.scc_smoother.active_cam:
if (self.frame -
self.last_active_cam_frame) * DT_CTRL > 10.0:
self.active_cam_timer = int(1.5 / DT_CTRL)
self.last_active_cam_frame = self.frame
if self.active_cam_timer > 0:
self.active_cam_timer -= 1
left_lane_warning = right_lane_warning = 1
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
if self.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
cut_steer_temp = False
if self.steer_fault_max_angle > 0:
if lkas_active and abs(
CS.out.steeringAngleDeg) > self.steer_fault_max_angle:
self.angle_limit_counter += 1
else:
self.angle_limit_counter = 0
# stop requesting torque to avoid 90 degree fault and hold torque with induced temporary fault
# two cycles avoids race conditions every few minutes
if self.angle_limit_counter > self.steer_fault_max_frames:
self.cut_steer = True
elif self.cut_steer_frames > 1:
self.cut_steer_frames = 0
self.cut_steer = False
if self.cut_steer:
cut_steer_temp = True
self.angle_limit_counter = 0
self.cut_steer_frames += 1
can_sends = []
can_sends.append(
hyundaican.create_lkas11(
self.packer, self.frame, self.CP.carFingerprint, apply_steer,
lkas_active, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning, 0, self.ldws_opt,
cut_steer_temp))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc is on bus 1
can_sends.append(
hyundaican.create_lkas11(
self.packer, self.frame, self.CP.carFingerprint,
apply_steer, lkas_active, CS.lkas11, sys_warning,
sys_state, CC.enabled, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, left_lane_warning,
right_lane_warning, 1, self.ldws_opt, cut_steer_temp))
if self.frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(
hyundaican.create_clu11(self.packer, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
if pcm_cancel_cmd and (self.longcontrol and not self.mad_mode_enabled):
can_sends.append(
hyundaican.create_clu11(self.packer, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
if CS.mdps_bus or self.CP.carFingerprint in FEATURES[
"send_mdps12"]: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(
hyundaican.create_mdps12(self.packer, self.frame, CS.mdps12))
self.update_auto_resume(CC, CS, clu11_speed, can_sends)
self.update_scc(CC, CS, actuators, controls, hud_control, can_sends)
# 20 Hz LFA MFA message
if self.frame % 5 == 0:
activated_hda = road_speed_limiter_get_active()
# activated_hda: 0 - off, 1 - main road, 2 - highway
if self.CP.carFingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(
hyundaican.create_lfahda_mfc(self.packer, CC.enabled,
activated_hda))
elif CS.has_lfa_hda:
can_sends.append(
hyundaican.create_hda_mfc(self.packer, activated_hda, CS,
hud_control.leftLaneVisible,
hud_control.rightLaneVisible))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.accel = self.accel
self.frame += 1
return new_actuators, can_sends
