class CarController():
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.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.turning_signal_timer = 0
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.scc_smoother = SccSmoother(accel_gain=1.0, decel_gain=1.0, curvature_gain=0.8)
def update(self, 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):
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# 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
# SPAS limit angle extremes for safety
if CS.spas_enabled:
apply_steer_ang_req = clip(actuators.steerAngle, -1 * (STEER_ANG_MAX), STEER_ANG_MAX)
# SPAS limit angle rate for safety
if abs(self.apply_steer_ang - apply_steer_ang_req) > STEER_ANG_MAX_RATE:
if apply_steer_ang_req > self.apply_steer_ang:
self.apply_steer_ang += STEER_ANG_MAX_RATE
else:
self.apply_steer_ang -= STEER_ANG_MAX_RATE
else:
self.apply_steer_ang = apply_steer_ang_req
spas_active = CS.spas_enabled and enabled and (self.spas_always or CS.out.vEgo < 7.0) # 25km/h
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 90. and not spas_active
# 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
# 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 and CS.out.vEgo < 2 * CV.KPH_TO_MS: # 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_accel_last = apply_accel
self.apply_steer_last = apply_steer
sys_warning, sys_state, left_lane_warning, right_lane_warning = \
process_hud_alert(lkas_active, 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 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"]
self.scc12_cnt = CS.scc12["CR_VSM_Alive"] + 1 if not CS.no_radar else 0
# 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
self.scc12_cnt %= 0xF
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))
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))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(create_clu11(self.packer, frame % 0x10, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed))
if pcm_cancel_cmd and self.longcontrol:
can_sends.append(create_clu11(self.packer, frame % 0x10, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
# fix auto resume - by neokii
if CS.out.cruiseState.standstill:
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 CS.lead_distance != self.last_lead_distance:
can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
if self.resume_cnt >= 8:
self.resume_cnt = 0
self.resume_wait_timer = SccSmoother.get_wait_count()
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
# scc smoother
if not self.longcontrol:
self.scc_smoother.update(enabled, can_sends, self.packer, CC, CS, frame, apply_accel, controls)
if CS.mdps_bus: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
# send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
if self.longcontrol and (CS.scc_bus or not self.scc_live) and frame % 2 == 0:
can_sends.append(create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt, self.scc_live, CS.scc12))
can_sends.append(create_scc11(self.packer, frame, enabled, set_speed, lead_visible, self.scc_live, CS.scc11))
if CS.has_scc13 and frame % 20 == 0:
can_sends.append(create_scc13(self.packer, CS.scc13))
if CS.has_scc14:
can_sends.append(create_scc14(self.packer, enabled, CS.scc14))
self.scc12_cnt += 1
# 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, lkas_active))
if CS.spas_enabled:
if CS.mdps_bus:
can_sends.append(create_ems11(self.packer, CS.ems11, spas_active))
# SPAS11 50hz
if (frame % 2) == 0:
if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7:
self.en_spas = 7
self.en_cnt = 0
if self.en_spas == 7 and self.en_cnt >= 8:
self.en_spas = 3
self.en_cnt = 0
if self.en_cnt < 8 and spas_active:
self.en_spas = 4
elif self.en_cnt >= 8 and spas_active:
self.en_spas = 5
if not spas_active:
self.apply_steer_ang = CS.mdps11_strang
self.en_spas = 3
self.en_cnt = 0
self.mdps11_stat_last = CS.mdps11_stat
self.en_cnt += 1
can_sends.append(create_spas11(self.packer, self.car_fingerprint, (frame // 2), self.en_spas, self.apply_steer_ang, CS.mdps_bus))
# SPAS12 20Hz
if (frame % 5) == 0:
can_sends.append(create_spas12(CS.mdps_bus))
return can_sends
class CarController():
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.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.turning_signal_timer = 0
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
self.mad_mode_enabled = Params().get('MadModeEnabled') == b'1'
self.scc_smoother = SccSmoother(accel_gain=1.0,
decel_gain=1.0,
curvature_gain=1.0)
def update(self, 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):
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE,
CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
# Steering Torque
new_steer = int(round(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.steeringAngleDeg) < 90.
# 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
# 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 and CS.out.vEgo < 2 * CV.KPH_TO_MS: # 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_accel_last = apply_accel
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 or not lkas_active:
enabled_speed = clu11_speed
controls.clu_speed_ms = clu11_speed * CV.KPH_TO_MS
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"]
self.scc12_cnt = CS.scc12[
"CR_VSM_Alive"] + 1 if not CS.no_radar else 0
# 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
self.scc12_cnt %= 0xF
# apply_steer = interp(CS.out.vEgo, [0., 3.], [0., 1.])
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))
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))
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, frame // 2 % 0x10, 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, frame % 0x10, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
# fix auto resume - by neokii
if CS.out.cruiseState.standstill:
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 CS.lead_distance != self.last_lead_distance:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
if self.resume_cnt >= 8:
self.resume_cnt = 0
self.resume_wait_timer = SccSmoother.get_wait_count()
# 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, apply_accel, controls)
if CS.mdps_bus: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
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
# 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) and frame % 2 == 0:
apply_accel, lead_drel = self.scc_smoother.get_fused_accel(
apply_accel, aReqValue, controls.sm)
controls.fused_accel = apply_accel
controls.lead_drel = lead_drel
can_sends.append(
create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt,
self.scc_live, CS.scc12))
can_sends.append(
create_scc11(self.packer, frame, enabled, set_speed,
controls.sm['radarState'], self.scc_live,
CS.scc11))
if CS.has_scc13 and frame % 20 == 0:
can_sends.append(create_scc13(self.packer, CS.scc13))
if CS.has_scc14:
can_sends.append(create_scc14(self.packer, enabled, CS.scc14))
self.scc12_cnt += 1
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfahda_mfc(self.packer, enabled))
return can_sends
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.params = CarControllerParams(CP)
self.packer = CANPacker(dbc_name)
self.frame = 0
self.apply_steer_last = 0
self.last_button_frame = 0
self.accel = 0
self.lkas11_cnt = 0
self.scc12_cnt = -1
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.turning_signal_timer = 0
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
self.turning_indicator_alert = False
param = Params()
self.mad_mode_enabled = param.get_bool('MadModeEnabled')
self.ldws_opt = param.get_bool('IsLdwsCar')
self.stock_navi_decel_enabled = param.get_bool('StockNaviDecelEnabled')
self.keep_steering_turn_signals = param.get_bool(
'KeepSteeringTurnSignals')
self.haptic_feedback_speed_camera = param.get_bool(
'HapticFeedbackWhenSpeedCamera')
self.scc_smoother = SccSmoother()
self.last_blinker_frame = 0
self.prev_active_cam = False
self.active_cam_timer = 0
self.last_active_cam_frame = 0
self.angle_limit_counter = 0
self.cut_steer_frames = 0
self.cut_steer = False
self.steer_fault_max_angle = CP.steerFaultMaxAngle
self.steer_fault_max_frames = CP.steerFaultMaxFrames
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
def update_auto_resume(self, CC, CS, clu11_speed, can_sends):
# 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
elif self.scc_smoother.is_active(self.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(
hyundaican.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)
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
def update_scc(self, CC, CS, actuators, controls, hud_control, can_sends):
# scc smoother
self.scc_smoother.update(CC.enabled, can_sends, self.packer, CC, CS,
self.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 self.frame % 2 == 0:
set_speed = hud_control.setSpeed
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
stopping = controls.LoC.long_control_state == LongCtrlState.stopping
apply_accel = self.scc_smoother.get_apply_accel(
CS, controls.sm, actuators.accel, stopping)
apply_accel = clip(apply_accel if CC.longActive else 0,
CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
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(
hyundaican.create_scc12(
self.packer, apply_accel, CC.enabled, self.scc12_cnt,
self.scc_live, CS.scc12, CS.out.gasPressed,
CS.out.brakePressed, CS.out.cruiseState.standstill,
self.CP.carFingerprint))
can_sends.append(
hyundaican.create_scc11(self.packer, self.frame,
CC.enabled, set_speed,
hud_control.leadVisible,
self.scc_live, CS.scc11,
self.scc_smoother.active_cam,
stock_cam))
if self.frame % 20 == 0 and CS.has_scc13:
can_sends.append(
hyundaican.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(
hyundaican.create_scc14(self.packer, CC.enabled,
CS.out.vEgo, acc_standstill,
apply_accel, CS.out.gasPressed,
obj_gap, CS.scc14))
else:
self.scc12_cnt = -1
def update_hda2(self, CC, CS):
actuators = CC.actuators
# 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)
if not CC.latActive:
apply_steer = 0
self.apply_steer_last = apply_steer
can_sends = []
# 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
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
class CarController():
def __init__(self, dbc_name, CP, VM):
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.apply_steer_last = 0
self.accel = 0
self.lkas11_cnt = 0
self.scc12_cnt = -1
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.turning_signal_timer = 0
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
self.turning_indicator_alert = False
param = Params()
self.mad_mode_enabled = param.get_bool('MadModeEnabled')
self.ldws_opt = param.get_bool('IsLdwsCar')
self.stock_navi_decel_enabled = param.get_bool('StockNaviDecelEnabled')
self.keep_steering_turn_signals = param.get_bool(
'KeepSteeringTurnSignals')
self.haptic_feedback_speed_camera = param.get_bool(
'HapticFeedbackWhenSpeedCamera')
self.scc_smoother = SccSmoother()
self.last_blinker_frame = 0
self.prev_active_cam = False
self.active_cam_timer = 0
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
class CarController():
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 = -1
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.turning_signal_timer = 0
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
self.mad_mode_enabled = Params().get_bool('MadModeEnabled')
self.ldws_opt = Params().get_bool('IsLdwsCar')
self.stock_navi_decel_enabled = Params().get_bool(
'StockNaviDecelEnabled')
# gas_factor, brake_factor
# Adjust it in the range of 0.7 to 1.3
self.scc_smoother = SccSmoother()
def update(self, 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):
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.accel / CarControllerParams.ACCEL_SCALE
apply_accel, self.accel_steady = accel_hysteresis(
apply_accel, self.accel_steady)
apply_accel = self.scc_smoother.get_accel(CS, controls.sm, apply_accel)
apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE,
CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
# 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)
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.steeringAngleDeg) < CS.CP.maxSteeringAngleDeg
# 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
# 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_accel_last = apply_accel
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 or not lkas_active:
enabled_speed = clu11_speed
controls.clu_speed_ms = clu11_speed * CS.speed_conv_to_ms
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, frame // 2 % 0x10, 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, frame % 0x10, 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.01:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
if self.resume_cnt >= 8:
self.resume_cnt = 0
self.resume_wait_timer = SccSmoother.get_wait_count() * 2
# 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, apply_accel, controls)
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
# 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:
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:
if CS.out.vEgo < 2.:
long_control_state = controls.LoC.long_control_state
acc_standstill = True if long_control_state == LongCtrlState.stopping else False
else:
acc_standstill = 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.mdps_bus == 0:
state = 2 if self.car_fingerprint in FEATURES[
"send_hda_state_2"] else 1
can_sends.append(
create_hda_mfc(self.packer, activated_hda, state))
return can_sends