def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
### 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
if not enabled:
apply_steer = 0
steer_req = 1 if enabled else 0
self.apply_steer_last = apply_steer
can_sends = []
self.lkas11_cnt = self.cnt % 0x10
self.clu11_cnt = self.cnt % 0x10
can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt,
enabled, CS.lkas11, hud_alert, keep_stock=True))
if pcm_cancel_cmd:
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5:
self.last_resume_cnt = self.cnt
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))
self.cnt += 1
return can_sends
def update(self, enabled, CS, frame, actuators):
""" Controls thread """
can_sends = []
### STEER ###
if enabled:
# calculate steer and also set limits due to driver 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
if CS.out.standstill and frame % 5 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
# TODO: improve the resume trigger logic by looking at actual radar data
can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.RESUME))
else:
apply_steer = 0
self.steer_rate_limited = False
if CS.out.cruiseState.enabled and frame % 20 == 0:
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 5hz until we sync with stock ACC state
can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.CANCEL))
self.apply_steer_last = apply_steer
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
if not enabled:
apply_steer = 0.
if self.car_fingerprint in (CAR.OUTBACK, CAR.LEGACY):
# add noise to prevent lkas fault from constant torque value for over 1s
if enabled and apply_steer == self.apply_steer_last:
self.counter = +1
if self.counter == 50:
apply_steer = round(int(apply_steer * 0.99))
else:
self.counter = 0
can_sends.append(
subarucan.create_steering_control(self.packer,
CS.CP.carFingerprint,
apply_steer, frame,
P.STEER_STEP))
self.apply_steer_last = apply_steer
if self.car_fingerprint == CAR.IMPREZA:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(
subarucan.create_es_distance(self.packer,
CS.es_distance_msg,
pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(
subarucan.create_es_lkas(self.packer, CS.es_lkas_msg,
visual_alert, left_line,
right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
if self.car_fingerprint in (CAR.OUTBACK,
CAR.LEGACY) and pcm_cancel_cmd:
can_sends.append(subarucan.create_door_control(self.packer))
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):
self.lfa_available = True if self.car_fingerprint in FEATURES[
"send_lfa_mfa"] else False
self.high_steer_allowed = True if self.car_fingerprint in FEATURES[
"allow_high_steer"] else False
# 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.)
or self.high_steer_allowed)
# 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
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)
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,
self.lfa_available))
if pcm_cancel_cmd:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# send resume at a max freq of 5Hz
if (frame - self.last_resume_frame) * DT_CTRL > 0.2:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.RES_ACCEL))
self.last_resume_frame = frame
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.lfa_available:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
return can_sends
def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd,
hud_alert):
if not self.enable_camera:
return
### Steering Torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_driver,
SteerLimitParams)
if not enabled:
apply_steer = 0
steer_req = 1 if enabled else 0
self.apply_steer_last = apply_steer
can_sends = []
self.lkas11_cnt = self.cnt % 0x10
self.clu11_cnt = self.cnt % 0x10
if self.camera_disconnected:
if (self.cnt % 10) == 0:
can_sends.append(create_lkas12())
if (self.cnt % 50) == 0:
can_sends.append(create_1191())
if (self.cnt % 7) == 0:
can_sends.append(create_1156())
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
apply_steer,
steer_req,
self.lkas11_cnt,
enabled,
CS.lkas11,
hud_alert,
keep_stock=(not self.camera_disconnected)))
if pcm_cancel_cmd:
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5:
self.last_resume_cnt = self.cnt
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))
### Send messages to canbus
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
self.cnt += 1
def update(self, c, CS, frame):
can_sends = []
apply_steer = 0
self.steer_rate_limited = False
if c.enabled:
# calculate steer and also set limits due to driver torque
new_steer = int(
round(c.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
if CS.out.standstill and frame % 5 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
# TODO: improve the resume trigger logic by looking at actual radar data
can_sends.append(
mazdacan.create_button_cmd(self.packer,
CS.CP.carFingerprint,
CS.crz_btns_counter,
Buttons.RESUME))
if c.cruiseControl.cancel or (CS.out.cruiseState.enabled
and not c.enabled):
# if brake is pressed, let us wait >20ms before trying to disable crz to avoid
# a race condition with the stock system, where the second cancel from openpilot
# will disable the crz 'main on'
self.brake_counter = self.brake_counter + 1
if frame % 20 == 0 and not (CS.out.brakePressed
and self.brake_counter < 3):
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 5hz until we sync with stock ACC state
can_sends.append(
mazdacan.create_button_cmd(self.packer,
CS.CP.carFingerprint,
CS.crz_btns_counter,
Buttons.CANCEL))
else:
self.brake_counter = 0
self.apply_steer_last = apply_steer
# send HUD alerts
if frame % 50 == 0:
ldw = c.hudControl.visualAlert == VisualAlert.ldw
steer_required = c.hudControl.visualAlert == VisualAlert.steerRequired
can_sends.append(
mazdacan.create_alert_command(self.packer, CS.cam_laneinfo,
ldw, steer_required))
# send steering command
can_sends.append(
mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
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 = 0
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)
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))
if pcm_cancel_cmd:
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, frame, CS.clu11, Buttons.RES_ACCEL))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.clu11_cnt = 0
# 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 == CAR.SONATA:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
return can_sends
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
### Steering Torque
apply_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(apply_steer,
self.apply_steer_last,
CS.steer_torque_driver,
SteerLimitParams)
if not enabled:
apply_steer = 0
steer_req = 1 if enabled else 0
self.apply_steer_last = apply_steer
can_sends = []
self.lkas11_cnt = self.cnt % 0x10
self.clu11_cnt = self.cnt % 0x10
self.mdps12_cnt = self.cnt % 0x100
if self.camera_disconnected:
if (self.cnt % 10) == 0:
can_sends.append(create_lkas12())
if (self.cnt % 50) == 0:
can_sends.append(create_1191())
if (self.cnt % 7) == 0:
can_sends.append(create_1156())
else:
can_sends.append(
create_mdps12(self.packer, self.car_fingerprint,
self.mdps12_cnt, CS.mdps12, CS.lkas11))
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
apply_steer,
steer_req,
self.lkas11_cnt,
enabled,
CS.lkas11,
hud_alert,
keep_stock=(not self.camera_disconnected)))
#if pcm_cancel_cmd:
#can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL))
if CS.stopped and (self.cnt - self.last_resume_cnt) > 20:
if (self.cnt - self.last_resume_cnt) % 5 == 0:
self.last_resume_cnt = self.cnt
can_sends.append(
create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL,
self.clu11_cnt))
self.cnt += 1
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, dragonconf):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
apply_steer = 0
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(
enabled, dragonconf.dpLatCtrl, dragonconf.dpSteeringOnSignal,
blinker_on or frame < self.blinker_end_frame, apply_steer)
self.last_blinker_on = blinker_on
can_sends.append(
subarucan.create_steering_control(self.packer, apply_steer,
frame, P.STEER_STEP))
self.apply_steer_last = apply_steer
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(
subarucan.create_es_distance(self.packer, CS.es_distance_msg,
pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(
subarucan.create_es_lkas(self.packer, CS.es_lkas_msg,
visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
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 = int(round(actuators.steer * self.p.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
self.p)
self.steer_rate_limited = new_steer != apply_steer
# disable when temp fault is active
lkas_active = enabled and not CS.out.steerWarning
# 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
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)
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))
if pcm_cancel_cmd:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# send resume at a max freq of 10Hz
if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([
create_clu11(self.packer, frame, CS.clu11,
Buttons.RES_ACCEL)
] * 25)
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, CAR.KIA_NIRO_EV,
CAR.KONA_EV, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED,
CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021
]:
can_sends.append(create_lfahda_mfc(self.packer, enabled))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
""" Controls thread """
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lat_ctrl, \
self.lane_change_enabled, \
self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled)
self.dp_last_modified = modified
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled
if not lkas_enabled:
apply_steer = 0
# dp
apply_steer = common_controller_ctrl(enabled,
self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal,
CS.out.leftBlinker,
CS.out.rightBlinker,
apply_steer)
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, P.STEER_STEP))
self.apply_steer_last = apply_steer
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
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, dragonconf):
# Steering Torque
new_steer = actuators.steer * self.p.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
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 not self.dp_hkg_smart_mdps and CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
lkas_active = False
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)
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(enabled,
dragonconf,
blinker_on or frame < self.blinker_end_frame,
apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
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))
if pcm_cancel_cmd:
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# send resume at a max freq of 10Hz
if (frame - self.last_resume_frame)*DT_CTRL > 0.1:
can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 20)
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, CAR.KIA_NIRO_EV, CAR.IONIQ_EV_2020]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
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
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)
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))
if pcm_cancel_cmd:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# 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:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11,
Buttons.RES_ACCEL))
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, enabled, CS, frame, actuators, pcm_cancel_cmd,
left_lane_visble, right_lane_visible): #TODO hud_alert
P = CarControllerParams
# *** compute control surfaces ***
# I THINK I GET IT NOW!!!
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed + 0.4 # for status message
moving_kinda_fast = CS.out.vEgo > CS.CP.minSteerSpeed # - 1.0 (i think the logic here was to keep the status bit on longer)
bad_to_bone = enabled and moving_fast
# magic_window: below minSteerSpeed but above cut-off logic
# enabled_below_minSteer = enabled and (CS.out.vEgo < CS.CP.minSteerSpeed) and (CS.out.vEgo > CS.CP.minSteerSpeed - 3.0) # 15.5 m/s cutoff
# if moving_fast or enabled_below_minSteer:
# moving_kinda_fast = True # moving_kinda_fast is the lkas active bit, right?
# elif CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
# moving_kinda_fast = False
# Calculate torque limits and ramp-up/ramp-down rates. If we fall below
# minSteerSpeed, ramp-down toward zero.
if bad_to_bone:
new_steer = int(round(actuators.steer * P.STEER_MAX))
else:
new_steer = 0
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
self.apply_steer_last = apply_steer
#*** control msgs ***
can_sends = []
if frame % P.STEER_STEP == 0:
can_sends.append(
create_lkas_command(self.packer, int(apply_steer),
moving_kinda_fast, frame))
if frame % P.HUD_STEP == 0:
can_sends.append(
create_lkas_hud_command(self.packer, enabled, left_lane_visble,
right_lane_visible))
# FIXME: restore cruise control button spam cancellation after we're further along in development
# if pcm_cancel_cmd:
# new_msg = create_wheel_buttons(CS.frame_23b)
# can_sends.append(new_msg)
return can_sends
def update(self, enabled, CS, frame, actuators, dragonconf):
""" Controls thread """
can_sends = []
### STEER ###
if enabled:
# calculate steer and also set limits due to driver 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
if CS.out.standstill and frame % 20 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button at 5hz if we're engaged at standstill to support full stop and go!
# TODO: improve the resume trigger logic by looking at actual radar data
can_sends.append(
mazdacan.create_button_cmd(self.packer,
CS.CP.carFingerprint,
Buttons.RESUME))
else:
apply_steer = 0
self.steer_rate_limited = False
if CS.out.cruiseState.enabled and frame % 20 == 0:
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 5hz until we sync with stock ACC state
can_sends.append(
mazdacan.create_button_cmd(self.packer,
CS.CP.carFingerprint,
Buttons.CANCEL))
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + dragonconf.dpSignalOffDelay
apply_steer = common_controller_ctrl(
enabled, dragonconf, blinker_on or frame < self.blinker_end_frame,
apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
self.apply_steer_last = apply_steer
can_sends.append(
mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
frame, apply_steer, CS.cam_lkas))
return can_sends
def update(self, CC, CS):
can_sends = []
apply_steer = 0
self.steer_rate_limited = False
if CC.latActive:
# calculate steer and also set limits due to driver torque
new_steer = int(round(CC.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
if CC.cruiseControl.cancel:
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
# a race condition with the stock system, where the second cancel from openpilot
# will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
# read 3 messages and most likely sync state before we attempt cancel.
self.brake_counter = self.brake_counter + 1
if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 10hz until we sync with stock ACC state
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
else:
self.brake_counter = 0
if CC.cruiseControl.resume and self.frame % 5 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button when planner wants car to move
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
self.apply_steer_last = apply_steer
# send HUD alerts
if self.frame % 50 == 0:
ldw = CC.hudControl.visualAlert == VisualAlert.ldw
steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
# TODO: find a way to silence audible warnings so we can add more hud alerts
steer_required = steer_required and CS.lkas_allowed_speed
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
# send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
self.frame, apply_steer, CS.cam_lkas))
new_actuators = CC.actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
self.frame += 1
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled and not CS.steer_not_allowed
if not lkas_enabled:
apply_steer = 0
can_sends.append(
subarucan.create_steering_control(self.packer,
CS.CP.carFingerprint,
apply_steer, frame,
P.STEER_STEP))
self.apply_steer_last = apply_steer
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(
subarucan.create_es_distance(self.packer, CS.es_distance_msg,
pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(
subarucan.create_es_lkas(self.packer, CS.es_lkas_msg,
visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
leftLaneVisible, rightLaneVisible): # TODO hud_alert
P = CarControllerParams
steer_ready = CS.out.vEgo > CS.CP.minSteerSpeed
if steer_ready:
self.steer_command_bit = True
if not steer_ready:
self.steer_command_bit = False
bad_to_bone = enabled and steer_ready
if bad_to_bone:
new_steer = int(round(actuators.steer * P.STEER_MAX))
else:
new_steer = 0
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
self.apply_steer_last = apply_steer
# *** control msgs ***
can_sends = []
# *** control msgs ***
# if pcm_cancel_cmd: # TODO: ENABLE ONCE STEERING WORKS
# new_msg = create_wheel_buttons(self.packer, self.frame, cancel=True)
# can_sends.append(new_msg)
counter = (frame / 2) % 16
if frame % 2 == 0:
can_sends.append(
create_lkas_command(self.packer, int(apply_steer), counter,
self.steer_command_bit))
if frame % 5 == 0:
can_sends.append(
create_lkas_hud(self.packer, enabled, leftLaneVisible,
rightLaneVisible))
return can_sends
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
def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
apply_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
lkas_enabled = enabled and not CS.steer_not_allowed
if not lkas_enabled:
apply_steer = 0
can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))
self.apply_steer_last = apply_steer
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, hud_v_cruise,
hud_show_lanes, hud_show_car, hud_alert):
P = self.params
# Send CAN commands.
can_sends = []
# STEER
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame // P.STEER_STEP) % 4
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
CanBus.POWERTRAIN, apply_steer,
idx, lkas_enabled))
# GAS/BRAKE
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(
round(interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame // 4) % 4
at_full_stop = enabled and CS.out.standstill
near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
can_sends.append(
gmcan.create_friction_brake_command(self.packer_ch,
CanBus.CHASSIS,
apply_brake, idx,
near_stop, at_full_stop))
at_full_stop = enabled and CS.out.standstill
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
CanBus.POWERTRAIN, apply_gas,
idx, enabled, at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(
gmcan.create_acc_dashboard_command(self.packer_pt,
CanBus.POWERTRAIN, enabled,
hud_v_cruise * CV.MS_TO_KPH,
hud_show_car, send_fcw))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(CanBus.OBSTACLE,
int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(self.packer_obj,
CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
CanBus.OBSTACLE, CS.out.vEgo, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
steer_alert = hud_alert == VisualAlert.steerRequired
can_sends.append(
gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status
return can_sends
def update(self, CC, CS, ext_bus):
actuators = CC.actuators
hud_control = CC.hudControl
can_sends = []
# **** Steering Controls ************************************************ #
if self.frame % P.HCA_STEP == 0:
# Logic to avoid HCA state 4 "refused":
# * Don't steer unless HCA is in state 3 "ready" or 5 "active"
# * Don't steer at standstill
# * Don't send > 3.00 Newton-meters torque
# * Don't send the same torque for > 6 seconds
# * Don't send uninterrupted steering for > 360 seconds
# One frame of HCA disabled is enough to reset the timer, without zeroing the
# torque value. Do that anytime we happen to have 0 torque, or failing that,
# when exceeding ~1/3 the 360 second timer.
if CC.latActive:
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
if apply_steer == 0:
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
self.hcaEnabledFrameCount += 1
if self.hcaEnabledFrameCount >= 118 * (100 /
P.HCA_STEP): # 118s
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
hcaEnabled = True
if self.apply_steer_last == apply_steer:
self.hcaSameTorqueCount += 1
if self.hcaSameTorqueCount > 1.9 * (
100 / P.HCA_STEP): # 1.9s
apply_steer -= (1, -1)[apply_steer < 0]
self.hcaSameTorqueCount = 0
else:
self.hcaSameTorqueCount = 0
else:
hcaEnabled = False
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (self.frame / P.HCA_STEP) % 16
can_sends.append(
volkswagencan.create_mqb_steering_control(
self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled))
# **** HUD Controls ***************************************************** #
if self.frame % P.LDW_STEP == 0:
if hud_control.visualAlert in (VisualAlert.steerRequired,
VisualAlert.ldw):
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
else:
hud_alert = MQB_LDW_MESSAGES["none"]
can_sends.append(
volkswagencan.create_mqb_hud_control(
self.packer_pt, CANBUS.pt, CC.enabled,
CS.out.steeringPressed, hud_alert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
CS.ldw_stock_values, hud_control.leftLaneDepart,
hud_control.rightLaneDepart))
# **** ACC Button Controls ********************************************** #
# FIXME: this entire section is in desperate need of refactoring
if self.CP.pcmCruise:
if self.frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
if CC.cruiseControl.cancel:
# Cancel ACC if it's engaged with OP disengaged.
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["cancel"] = True
elif CC.cruiseControl.resume:
# Send Resume button when planner wants car to move
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["resumeCruise"] = True
if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
self.graMsgBusCounterPrev = CS.graMsgBusCounter
if self.graButtonStatesToSend is not None:
if self.graMsgSentCount == 0:
self.graMsgStartFramePrev = self.frame
idx = (CS.graMsgBusCounter + 1) % 16
can_sends.append(
volkswagencan.create_mqb_acc_buttons_control(
self.packer_pt, ext_bus,
self.graButtonStatesToSend, CS, idx))
self.graMsgSentCount += 1
if self.graMsgSentCount >= P.GRA_VBP_COUNT:
self.graButtonStatesToSend = None
self.graMsgSentCount = 0
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / P.STEER_MAX
self.frame += 1
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, 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.steer_torque_driver,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
### LKAS button to temporarily disable steering
if not CS.lkas_error:
if CS.lkas_button_on != self.lkas_button_last:
self.lkas_button = not self.lkas_button
self.lkas_button_last = CS.lkas_button_on
if self.car_fingerprint == CAR.GENESIS:
lkas_active = enabled and abs(
CS.angle_steers) < 90. and not self.lkas_button
else:
lkas_active = enabled and not self.lkas_button
# Fix for sharp turns mdps fault and Genesis hard fault at low speed
if CS.v_ego < 15.4 and self.car_fingerprint == CAR.GENESIS and not CS.mdps_bus:
self.turning_signal_timer = 100
if ((CS.left_blinker_flash or CS.right_blinker_flash)
and (CS.steer_override or abs(CS.angle_steers) > 15.)
and CS.v_ego < 15.0):
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
if self.turning_signal_timer:
lkas_active = 0
self.turning_signal_timer -= 1
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
hud_alert, lane_visible, left_lane_warning, right_lane_warning =\
process_hud_alert(lkas_active, self.lkas_button, self.car_fingerprint, visual_alert,
left_line, right_line,left_lane_depart, right_lane_depart)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 35 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
can_sends = []
lkas11_cnt = frame % 0x10
clu11_cnt = frame % 0x10
mdps12_cnt = frame % 0x100
self.scc12_cnt %= 15
can_sends.append(
create_lkas11(self.packer,
self.car_fingerprint,
0,
apply_steer,
steer_req,
lkas11_cnt,
lkas_active,
CS.lkas11,
hud_alert,
lane_visible,
left_lane_depart,
right_lane_depart,
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,
lkas11_cnt,
lkas_active,
CS.lkas11,
hud_alert,
lane_visible,
left_lane_depart,
right_lane_depart,
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, 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, 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, mdps12_cnt,
CS.mdps12))
if self.longcontrol and frame % 2:
can_sends.append(
create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt,
CS.scc12))
self.scc12_cnt += 1
if CS.stopped:
# 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
return can_sends
def update(self, enabled, CS, frame, actuators, hud_v_cruise,
hud_show_lanes, hud_show_car, hud_alert, paddleFrame):
##print("paddleFrame %d", paddleFrame)
P = self.params
# Send CAN commands.
can_sends = []
# STEER
if (frame % P.STEER_STEP) == 0:
#lkas_enabled = enabled and not CS.out.steerWarning and CS.lkMode and CS.out.vEgo > P.MIN_STEER_SPEED
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED
if lkas_enabled:
new_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame // P.STEER_STEP) % 4
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
CanBus.POWERTRAIN, apply_steer,
idx, lkas_enabled))
# GAS/BRAKE
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(
round(interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame // 4) % 4
## 인게이지 상태 아니고 오토홀드 스위치 들어가 있고 엑셀 페달 밟는 상태가 아니고 기어가 d,l에 있으며 속도가 1.8kph 이하일때 동작
##if not enabled and CS.autoHold and not CS.out.gasPressed and CS.out.gearShifter == 'drive' and CS.out.vEgo < 0.01 and not CS.regenPaddlePressed:
if not enabled and CS.autoHold and CS.autoHoldActive and not CS.out.gasPressed and CS.out.gearShifter == 'drive' and CS.out.vEgo < 0.01 and not CS.regenPaddlePressed:
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.out.vEgo < 0.01 ## neokii test
at_full_stop = standstill and car_stopping
near_stop = (CS.out.vEgo <
P.NEAR_STOP_BRAKE_PHASE) and car_stopping
##if at_full_stop :
##print("A------------------")
##print("apply_brake", apply_brake)
##print("car_stopping", car_stopping)
##print("apply_gas ", apply_gas)
##print("P.ZERO_GAS ", P.ZERO_GAS)
##print("standstill ", standstill)
##print("CS.pcm_acc_status ", CS.pcm_acc_status)
##print("AccState.STANDSTILL ", AccState.STANDSTILL)
##print("at_full_stop", at_full_stop)
##print("near_stop ", near_stop)
##print("CS.out.vEgo ", CS.out.vEgo)
##print(" P.NEAR_STOP_BRAKE_PHASE ", P.NEAR_STOP_BRAKE_PHASE)
##print("enabled ", enabled)
#print("CS.autoHold ", CS.autoHold)
#print("CS.autoHoldActive ", CS.autoHoldActive)
##print("CS.gas", CS.out.gas)
##print("CS.gasPressed", CS.out.gasPressed)
##print("gearShifter ", CS.out.gearShifter)
####at_full_stop = standstill and car_stopping
####can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop))
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
near_stop, at_full_stop))
CS.autoHoldActivated = True
else:
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = enabled and standstill and car_stopping
near_stop = enabled and (
CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
##print("B------------------")
##print("apply_brake", apply_brake)
##print("car_stopping", car_stopping)
##print("apply_gas ", apply_gas)
##print("P.ZERO_GAS ", P.ZERO_GAS)
##print("standstill ", standstill)
##print("CS.pcm_acc_status ", CS.pcm_acc_status)
##print("AccState.STANDSTILL ", AccState.STANDSTILL)
##print("at_full_stop", at_full_stop)
##print("near_stop ", near_stop)
##print("CS.out.vEgo ", CS.out.vEgo)
##print(" P.NEAR_STOP_BRAKE_PHASE ", P.NEAR_STOP_BRAKE_PHASE)
##print("enabled ", enabled)
#print("CS.autoHold ", CS.autoHold)
#print("CS.autoHoldActive ", CS.autoHoldActive)
##print("CS.gas", CS.out.gas)
##print("CS.gasPressed", CS.out.gasPressed)
##print("gearShifter ", CS.out.gearShifter)
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, CanBus.CHASSIS, apply_brake, idx,
near_stop, at_full_stop))
CS.autoHoldActivated = False
# Auto-resume from full stop by resetting ACC control
acc_enabled = enabled
if standstill and not car_stopping:
acc_enabled = False
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
CanBus.POWERTRAIN,
apply_gas, idx, acc_enabled,
at_full_stop))
follow_level = CS.get_follow_level()
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
send_fcw = hud_alert == VisualAlert.fcw
can_sends.append(
gmcan.create_acc_dashboard_command(self.packer_pt,
CanBus.POWERTRAIN, enabled,
hud_v_cruise * CV.MS_TO_KPH,
hud_show_car, send_fcw,
follow_level))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(CanBus.OBSTACLE,
int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(self.packer_obj,
CanBus.OBSTACLE))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame // speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
CanBus.OBSTACLE, CS.out.vEgo, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last:
steer_alert = hud_alert == VisualAlert.steerRequired
can_sends.append(
gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active,
lka_critical, steer_alert))
self.lka_icon_status_last = lka_icon_status
return can_sends
def update(self, CC, CS):
actuators = CC.actuators
hud_control = CC.hudControl
# 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)
self.steer_rate_limited = new_steer != apply_steer
if not CC.latActive:
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.car_fingerprint,
hud_control)
can_sends = []
if self.CP.carFingerprint in HDA2_CAR:
# steering control
can_sends.append(hda2can.create_lkas(self.packer, CC.enabled, self.frame, CC.latActive, apply_steer))
# cruise cancel
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
if CC.cruiseControl.cancel:
for _ in range(20):
can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, True, False))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, False, True))
self.last_button_frame = self.frame
else:
# tester present - w/ no response (keeps radar disabled)
if self.CP.openpilotLongitudinalControl:
if self.frame % 100 == 0:
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive,
CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
if not self.CP.openpilotLongitudinalControl:
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL))
elif CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
self.last_button_frame = self.frame
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
accel = actuators.accel
jerk = 0
if CC.longActive:
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
if accel < 0:
accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping, CS.out.gasPressed))
self.accel = accel
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(hyundaican.create_acc_opt(self.packer))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
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(self, sendcan, enabled, CS, frame, actuators, \
hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt):
""" Controls thread """
#update custom UI buttons and alerts
CS.UE.update_custom_ui()
if (frame % 1000 == 0):
CS.cstm_btns.send_button_info()
CS.UE.uiSetCarEvent(CS.cstm_btns.car_folder, CS.cstm_btns.car_name)
# Sanity check.
if not self.allow_controls:
return
P = self.params
# Send CAN commands.
can_sends = []
canbus = self.canbus
### STEER ###
if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
if lkas_enabled:
apply_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(
apply_steer, self.apply_steer_last, CS.steer_torque_driver,
P)
else:
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4
if CS.cstm_btns.get_button_status("lka") == 0:
apply_steer = 0
if self.car_fingerprint == CAR.VOLT:
can_sends.append(
gmcan.create_steering_control(self.packer_pt,
canbus.powertrain,
apply_steer, idx,
lkas_enabled))
if self.car_fingerprint == CAR.CADILLAC_CT6:
can_sends += gmcan.create_steering_control_ct6(
self.packer_pt, canbus, apply_steer, CS.v_ego, idx,
lkas_enabled)
### GAS/BRAKE ###
if self.car_fingerprint == CAR.VOLT:
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
# *** apply pedal hysteresis ***
final_brake, self.brake_steady = actuator_hystereses(
final_pedal, self.pedal_steady)
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(
round(interp(final_pedal, P.GAS_LOOKUP_BP,
P.GAS_LOOKUP_V)))
apply_brake = int(
round(
interp(final_pedal, P.BRAKE_LOOKUP_BP,
P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:
idx = (frame / 4) % 4
car_stopping = apply_gas < P.ZERO_GAS
standstill = CS.pcm_acc_status == AccState.STANDSTILL
at_full_stop = enabled and standstill and car_stopping
near_stop = enabled and (
CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
can_sends.append(
gmcan.create_friction_brake_command(
self.packer_ch, canbus.chassis, apply_brake, idx,
near_stop, at_full_stop))
# Auto-resume from full stop by resetting ACC control
acc_enabled = enabled
if standstill and not car_stopping:
acc_enabled = False
can_sends.append(
gmcan.create_gas_regen_command(self.packer_pt,
canbus.powertrain,
apply_gas, idx, acc_enabled,
at_full_stop))
# Send dashboard UI commands (ACC status), 25hz
if (frame % 4) == 0:
can_sends.append(
gmcan.create_acc_dashboard_command(
self.packer_pt, canbus.powertrain, enabled,
hud_v_cruise * CV.MS_TO_KPH, hud_show_car))
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
if frame % time_and_headlights_step == 0:
idx = (frame / time_and_headlights_step) % 4
can_sends.append(
gmcan.create_adas_time_status(
canbus.obstacle, int((tt - self.start_time) * 60),
idx))
can_sends.append(
gmcan.create_adas_headlights_status(canbus.obstacle))
speed_and_accelerometer_step = 2
if frame % speed_and_accelerometer_step == 0:
idx = (frame / speed_and_accelerometer_step) % 4
can_sends.append(
gmcan.create_adas_steering_status(canbus.obstacle, idx))
can_sends.append(
gmcan.create_adas_accelerometer_speed_status(
canbus.obstacle, CS.v_ego, idx))
if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last:
can_sends.append(
gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active,
lka_critical))
self.lka_icon_status_last = lka_icon_status
# Send chimes
if self.chime != chime:
duration = 0x3c
# There is no 'repeat forever' chime command
# TODO: Manage periodic re-issuing of chime command
# and chime cancellation
if chime_cnt == -1:
chime_cnt = 10
if chime != 0:
can_sends.append(
gmcan.create_chime_command(canbus.sw_gmlan, chime,
duration, chime_cnt))
# If canceling a repeated chime, cancel command must be
# issued for the same chime type and duration
self.chime = chime
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
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
def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert,
left_lane_visible, right_lane_visible, left_lane_depart,
right_lane_depart):
""" Controls thread """
can_sends = []
# **** Steering Controls ************************************************ #
if frame % P.HCA_STEP == 0:
# Logic to avoid HCA state 4 "refused":
# * Don't steer unless HCA is in state 3 "ready" or 5 "active"
# * Don't steer at standstill
# * Don't send > 3.00 Newton-meters torque
# * Don't send the same torque for > 6 seconds
# * Don't send uninterrupted steering for > 360 seconds
# One frame of HCA disabled is enough to reset the timer, without zeroing the
# torque value. Do that anytime we happen to have 0 torque, or failing that,
# when exceeding ~1/3 the 360 second timer.
if enabled and CS.out.vEgo > CS.CP.minSteerSpeed and not (
CS.out.standstill or CS.out.steerError
or CS.out.steerWarning):
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
if apply_steer == 0:
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
self.hcaEnabledFrameCount += 1
if self.hcaEnabledFrameCount >= 118 * (100 /
P.HCA_STEP): # 118s
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
hcaEnabled = True
if self.apply_steer_last == apply_steer:
self.hcaSameTorqueCount += 1
if self.hcaSameTorqueCount > 1.9 * (
100 / P.HCA_STEP): # 1.9s
apply_steer -= (1, -1)[apply_steer < 0]
self.hcaSameTorqueCount = 0
else:
self.hcaSameTorqueCount = 0
else:
hcaEnabled = False
apply_steer = 0
self.apply_steer_last = apply_steer
idx = (frame / P.HCA_STEP) % 16
can_sends.append(
volkswagencan.create_mqb_steering_control(
self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled))
# **** HUD Controls ***************************************************** #
if frame % P.LDW_STEP == 0:
if visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]:
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
else:
hud_alert = MQB_LDW_MESSAGES["none"]
can_sends.append(
volkswagencan.create_mqb_hud_control(
self.packer_pt, CANBUS.pt, enabled, CS.out.steeringPressed,
hud_alert, left_lane_visible, right_lane_visible,
CS.ldw_stock_values, left_lane_depart, right_lane_depart))
# **** ACC Button Controls ********************************************** #
# FIXME: this entire section is in desperate need of refactoring
if CS.CP.pcmCruise:
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
if not enabled and CS.out.cruiseState.enabled:
# Cancel ACC if it's engaged with OP disengaged.
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["cancel"] = True
elif enabled and CS.esp_hold_confirmation:
# Blip the Resume button if we're engaged at standstill.
# FIXME: This is a naive implementation, improve with visiond or radar input.
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["resumeCruise"] = True
if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
self.graMsgBusCounterPrev = CS.graMsgBusCounter
if self.graButtonStatesToSend is not None:
if self.graMsgSentCount == 0:
self.graMsgStartFramePrev = frame
idx = (CS.graMsgBusCounter + 1) % 16
can_sends.append(
volkswagencan.create_mqb_acc_buttons_control(
self.packer_pt, ext_bus,
self.graButtonStatesToSend, CS, idx))
self.graMsgSentCount += 1
if self.graMsgSentCount >= P.GRA_VBP_COUNT:
self.graButtonStatesToSend = None
self.graMsgSentCount = 0
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / P.STEER_MAX
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, left_lane_depart,
right_lane_depart):
can_sends = []
# *** steering ***
if (frame % self.p.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorque,
self.p)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
apply_steer = 0
if CS.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(
subarucan.create_preglobal_steering_control(
self.packer, apply_steer, frame, self.p.STEER_STEP))
else:
can_sends.append(
subarucan.create_steering_control(self.packer, apply_steer,
frame,
self.p.STEER_STEP))
self.apply_steer_last = apply_steer
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged
if pcm_cancel_cmd:
cruise_button = 1
# turn main on if off and past start-up state
elif not CS.out.cruiseState.available and CS.ready:
cruise_button = 1
else:
cruise_button = CS.cruise_button
# unstick previous mocked button press
if cruise_button == 1 and self.cruise_button_prev == 1:
cruise_button = 0
self.cruise_button_prev = cruise_button
can_sends.append(
subarucan.create_preglobal_es_distance(
self.packer, cruise_button, CS.es_distance_msg))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
else:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(
subarucan.create_es_distance(self.packer,
CS.es_distance_msg,
pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(
subarucan.create_es_lkas(self.packer, CS.es_lkas_msg,
enabled, visual_alert, left_line,
right_line, left_lane_depart,
right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
return new_actuators, can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
left_lane, right_lane, left_lane_depart, right_lane_depart):
# Steering Torque
new_steer = int(round(actuators.steer * self.p.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
self.steer_rate_limited = new_steer != apply_steer
# disable when temp fault is active, or below LKA minimum speed
lkas_active = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed
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)
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.openpilotLongitudinalControl:
if (frame % 100) == 0:
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
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))
if not CS.CP.openpilotLongitudinalControl:
if pcm_cancel_cmd:
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# send resume at a max freq of 10Hz
if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likelihood of resume being accepted
can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
self.last_resume_frame = frame
if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
lead_visible = False
accel = actuators.accel if enabled else 0
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
if accel < 0:
accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = (actuators.longControlState == LongCtrlState.stopping)
set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
self.accel = accel
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020]:
can_sends.append(create_lfahda_mfc(self.packer, enabled))
# 5 Hz ACC options
if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
can_sends.extend(create_acc_opt(self.packer))
# 2 Hz front radar options
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
can_sends.append(create_frt_radar_opt(self.packer))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.p.STEER_MAX
new_actuators.accel = self.accel
return new_actuators, can_sends
