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, 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, 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, enabled, CS, frame, actuators, visual_alert, pcm_cancel,
dragonconf):
can_sends = []
steer_alert = visual_alert in [
VisualAlert.steerRequired, VisualAlert.ldw
]
apply_steer = actuators.steer
# 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
if pcm_cancel:
#print "CANCELING!!!!"
can_sends.append(spam_cancel_button(self.packer))
if (frame % 3) == 0:
curvature = self.vehicle_model.calc_curvature(
actuators.steeringAngleDeg * math.pi / 180., CS.out.vEgo)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
self.lkas_action += int(CS.out.genericToggle
and not self.generic_toggle_last)
self.lkas_action &= 0xf
else:
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
can_sends.append(
create_steer_command(self.packer, apply_steer, enabled,
CS.lkas_state, CS.out.steeringAngleDeg,
curvature, self.lkas_action))
self.generic_toggle_last = CS.out.genericToggle
if (frame % 100) == 0:
can_sends.append(
make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \
(self.steer_alert_last != steer_alert):
can_sends.append(
create_lkas_ui(self.packer, CS.out.cruiseState.available,
enabled, steer_alert))
if (frame % 200) == 0:
can_sends.append(
make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
if (frame % 10) == 0:
can_sends.append(
make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1))
can_sends.append(
make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1))
can_sends.append(
make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1))
can_sends.append(
make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1))
can_sends.append(
make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1))
can_sends.append(
make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1))
can_sends.append(
make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1))
can_sends.append(
make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1))
can_sends.append(
make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1))
can_sends.append(
make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1))
static_msgs = range(1653, 1658)
for addr in static_msgs:
cnt = (frame % 10) + 1
can_sends.append(
make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') +
b'\x00\x00\x00\x00\x00\x00\x00', 1))
self.enabled_last = enabled
self.main_on_last = CS.out.cruiseState.available
self.steer_alert_last = steer_alert
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_line, right_line, left_lane_depart,
right_lane_depart, dragonconf):
can_sends = []
# *** steering ***
if (frame % CarControllerParams.STEER_STEP) == 0:
apply_steer = int(
round(actuators.steer * CarControllerParams.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,
CarControllerParams)
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, blinker_on
or frame < self.blinker_end_frame, apply_steer, CS.out.vEgo)
self.last_blinker_on = blinker_on
if CS.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(
subarucan.create_preglobal_steering_control(
self.packer, apply_steer, frame,
CarControllerParams.STEER_STEP))
else:
can_sends.append(
subarucan.create_steering_control(
self.packer, apply_steer, frame,
CarControllerParams.STEER_STEP))
self.apply_steer_last = apply_steer
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_accel_cnt != CS.es_accel_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_es_throttle_control(
self.packer, cruise_button, CS.es_accel_msg))
self.es_accel_cnt = CS.es_accel_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,
visual_alert, left_line,
right_line, left_lane_depart,
right_lane_depart))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
def update(self, enabled, CS, frame, actuators, visual_alert, audible_alert, leftLaneVisible, rightLaneVisible, dragonconf):
""" Controls thread """
P = CarControllerParams
# Send CAN commands.
can_sends = []
#--------------------------------------------------------------------------
# #
# Prepare HCA_01 Heading Control Assist messages with steering torque. #
# #
#--------------------------------------------------------------------------
# The factory camera sends at 50Hz while steering and 1Hz when not. When
# OP is active, Panda filters HCA_01 from the factory camera and OP emits
# HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and
# doesn't seem to add value at this time. The rack will accept HCA_01 at
# 100Hz if we want to control at finer resolution in the future.
if frame % P.HCA_STEP == 0:
# FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop
# commanding HCA if there's a fault, so the steering rack recovers.
if enabled and not (CS.out.standstill or CS.steeringFault):
# FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This
# is inherently handled by scaling to STEER_MAX. The rack doesn't seem
# to care about up/down rate, but we have some evidence it may do its
# own rate limiting, and matching OP helps for accurate tuning.
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
# FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending
# a single frame with HCA disabled is an effective workaround.
if apply_steer == 0:
# We can usually reset the timer for free, just by disabling HCA
# when apply_steer is exactly zero, which happens by chance during
# many steer torque direction changes. This could be expanded with
# a small dead-zone to capture all zero crossings, but not seeing a
# major need at this time.
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
self.hcaEnabledFrameCount += 1
if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s
# The Kansas I-70 Crosswind Problem: if we truly do need to steer
# in one direction for > 360 seconds, we have to disable HCA for a
# frame while actively steering. Testing shows we can just set the
# disabled flag, and keep sending non-zero torque, which keeps the
# Panda torque rate limiting safety happy. Do so 3x within the 360
# second window for safety and redundancy.
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
hcaEnabled = True
# FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds.
# This is to detect the sending camera being stuck or frozen. OP
# can trip this on a curve if steering is saturated. Avoid this by
# reducing torque 0.01 Nm for one frame. Do so 3x within the 6
# second period for safety and redundancy.
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:
# Continue sending HCA_01 messages, with the enable flags turned off.
hcaEnabled = False
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
self.apply_steer_last = apply_steer
idx = (frame / P.HCA_STEP) % 16
can_sends.append(self.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer,
idx, hcaEnabled))
#--------------------------------------------------------------------------
# #
# Prepare LDW_02 HUD messages with lane borders, confidence levels, and #
# the LKAS status LED. #
# #
#--------------------------------------------------------------------------
# The factory camera emits this message at 10Hz. When OP is active, Panda
# filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz.
if frame % P.LDW_STEP == 0:
hcaEnabled = True if enabled and not CS.out.standstill else False
if visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired:
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
else:
hud_alert = MQB_LDW_MESSAGES["none"]
can_sends.append(self.create_hud_control(self.packer_pt, CANBUS.pt, hcaEnabled,
CS.out.steeringPressed, hud_alert, leftLaneVisible,
rightLaneVisible))
#--------------------------------------------------------------------------
# #
# Prepare GRA_ACC_01 ACC control messages with button press events. #
# #
#--------------------------------------------------------------------------
# The car sends this message at 33hz. OP sends it on-demand only for
# virtual button presses.
#
# First create any virtual button press event needed by openpilot, to sync
# stock ACC with OP disengagement, or to auto-resume from stop.
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.out.standstill:
# Blip the Resume button if we're engaged at standstill.
# FIXME: This is a naive implementation, improve with visiond or radar input.
# A subset of MQBs like to "creep" too aggressively with this implementation.
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["resumeCruise"] = True
# OP/Panda can see this message but can't filter it when integrated at the
# R242 LKAS camera. It could do so if integrated at the J533 gateway, but
# we need a generalized solution that works for either. The message is
# counter-protected, so we need to time our transmissions very precisely
# to achieve fast and fault-free switching between message flows accepted
# at the J428 ACC radar.
#
# Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP):
#
# CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6
# EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^
#
# If OP needs to send a button press, it waits to see a GRA_ACC_01 message
# counter change, and then immediately follows up with the next increment.
# The OP message will be sent within about 1ms of the car's message, which
# is about 2ms before the car's next message is expected. OP sends for an
# arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new
# message from the car.
#
# Because OP's counter is synced to the car, J428 immediately accepts the
# OP messages as valid. Further messages from the car get discarded as
# duplicates without a fault. When OP stops sending, the extra time gap
# (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428
# tolerates the gap just fine and control returns to the car immediately.
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(self.create_acc_buttons_control(self.packer_pt, self.acc_bus, self.graButtonStatesToSend, CS, idx))
self.graMsgSentCount += 1
if self.graMsgSentCount >= P.GRA_VBP_COUNT:
self.graButtonStatesToSend = None
self.graMsgSentCount = 0
return can_sends
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert,
dragonconf):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return []
# *** compute control surfaces ***
# steer torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
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
self.apply_steer_last = apply_steer
can_sends = []
#*** control msgs ***
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.packer,
self.ccframe,
cancel=True)
can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
return can_sends
def update(self, enabled, CS, frame, actuators, hud_v_cruise,
hud_show_lanes, hud_show_car, hud_alert, dragonconf):
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
# 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
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))
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, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, hud_speed, left_lane, right_lane,
left_lane_depart, right_lane_depart, dragonconf):
# 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
# 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
# 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
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))
# 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
]:
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))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart, dragonconf):
# *** compute control surfaces ***
# gas and brake
interceptor_gas_cmd = 0.
pcm_accel_cmd = actuators.accel
if CS.CP.enableGasInterceptor:
# handle hysteresis when around the minimum acc speed
if CS.out.vEgo < MIN_ACC_SPEED:
self.use_interceptor = True
elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
self.use_interceptor = False
if self.use_interceptor and enabled:
# only send negative accel when using interceptor. gas handles acceleration
# +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED],
[0.2, 0.5])
interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0.,
MAX_INTERCEPTOR_GAS)
pcm_accel_cmd = 0.18 - max(0, -actuators.accel)
pcm_accel_cmd, self.accel_steady = accel_hysteresis(
pcm_accel_cmd, self.accel_steady, enabled)
pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN,
CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.last_steer, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in [
9, 25
] or abs(CS.out.steeringRateDeg) > 100 or (
abs(CS.out.steeringAngleDeg) > 150
and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not enabled and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if not dragonconf.dpToyotaSng and CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
# 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.last_steer = apply_steer
self.last_accel = pcm_accel_cmd
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(
create_steer_command(self.packer, apply_steer, apply_steer_req,
frame))
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
can_sends.append(
create_lta_steer_command(self.packer, 0, 0, frame // 2))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
if dragonconf.dpAtl and dragonconf.dpAtlOpLong and not CS.out.cruiseActualEnabled:
pcm_accel_cmd = 0.
if CS.CP.enableGasInterceptor:
interceptor_gas_cmd = 0.
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0
and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
if dragonconf.dpAtl and not dragonconf.dpAtlOpLong:
pass
# Lexus IS uses a different cancellation message
elif pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(
create_accel_command(self.packer, pcm_accel_cmd,
pcm_cancel_cmd, self.standstill_req,
lead, CS.acc_type, CS.distance))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, CS.acc_type, CS.distance))
if frame % 2 == 0 and CS.CP.enableGasInterceptor:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, interceptor_gas_cmd,
frame // 2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
# dp
if not dragonconf.dpToyotaLdw:
left_lane_depart = False
right_lane_depart = False
if (frame % 100 == 0 or send_ui):
can_sends.append(
create_ui_command(self.packer, steer_alert, pcm_cancel_cmd,
left_line, right_line, left_lane_depart,
right_lane_depart, enabled))
if frame % 100 == 0 and CS.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
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 = 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
# 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
# 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
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_speed, pcm_override,
pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes,
dragonconf, hud_show_car, hud_alert):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# Never send cancel command if we never enter cruise state (no cruise if pedal)
# Cancel cmd causes brakes to release at a standstill causing grinding
pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required,
CS.lkMode)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
if (frame % 10) == 0:
can_sends.append(
(0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
# 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
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(
self.packer, apply_steer, lkas_active, CS.CP.carFingerprint,
idx, CS.CP.openpilotLongitudinalControl))
# Send dashboard UI commands.
if not dragonconf.dpAtl and (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx))
if dragonconf.dpAtl:
pass
# If using stock ACC, spam cancel command to kill gas when OP disengages.
elif not dragonconf.dpAllowGas and pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH,
CAR.INSIGHT):
rough_lead_speed = self.rough_speed(CS.lead_distance)
if CS.lead_distance > (self.stopped_lead_distance +
15.0) or rough_lead_speed > 0.1:
self.stopped_lead_distance = 0.0
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
if CS.hud_lead == 1:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
else:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx,
CS.CP.carFingerprint))
else:
self.stopped_lead_distance = CS.lead_distance
self.prev_lead_distance = CS.lead_distance
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if dragonconf.dpAtl:
pass
elif CS.CP.carFingerprint in HONDA_BOSCH:
accel = actuators.gas - actuators.brake
# TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
stopping = accel < 0 and CS.out.vEgo < 0.3
starting = accel > 0 and CS.out.vEgo < 0.3
# Prevent rolling backwards
accel = -1.0 if stopping else accel
apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP,
P.BOSCH_ACCEL_LOOKUP_V)
apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP,
P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(
hondacan.create_acc_commands(self.packer, enabled,
apply_accel, apply_gas,
idx, stopping, starting,
CS.CP.carFingerprint))
else:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0,
P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.fcw,
idx,
CS.CP.carFingerprint,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
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, dragonconf):
""" 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 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
# dp
if CS.out.stopSteering:
apply_steer = 0
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
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_lane_warning_left, CS.ldw_lane_warning_right,
CS.ldw_side_dlc_tlc, CS.ldw_dlc, CS.ldw_tlc,
CS.out.standstill, left_lane_depart, right_lane_depart))
# **** ACC Button Controls ********************************************** #
# FIXME: this entire section is in desperate need of refactoring
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.out.standstill:
# Blip the Resume button if we're engaged at standstill.
# FIXME: This is a naive implementation, improve with visiond or radar input.
# A subset of MQBs like to "creep" too aggressively with this implementation.
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
return can_sends
def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
left_line, right_line, left_lane_depart, right_lane_depart,
dragonconf):
""" Controls thread """
# Send CAN commands.
can_sends = []
### STEER ###
acc_active = bool(CS.out.cruiseState.enabled)
lkas_hud_msg = CS.lkas_hud_msg
lkas_hud_info_msg = CS.lkas_hud_info_msg
apply_angle = actuators.steerAngle
steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0
if enabled:
# # windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
# Max torque from driver before EPS will give up and not apply torque
if not bool(CS.out.steeringPressed):
self.lkas_max_torque = LKAS_MAX_TORQUE
else:
# Scale max torque based on how much torque the driver is applying to the wheel
self.lkas_max_torque = max(
# Scale max torque down to half LKAX_MAX_TORQUE as a minimum
LKAS_MAX_TORQUE * 0.5,
# Start scaling torque at STEER_THRESHOLD
LKAS_MAX_TORQUE -
0.6 * max(0,
abs(CS.out.steeringTorque) - STEER_THRESHOLD))
else:
apply_angle = CS.out.steeringAngle
self.lkas_max_torque = 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_angle = common_controller_ctrl(
enabled, dragonconf.dpLatCtrl, dragonconf.dpSteeringOnSignal,
blinker_on or frame < self.blinker_end_frame, apply_angle)
self.last_blinker_on = blinker_on
self.last_angle = apply_angle
if not enabled and acc_active:
# send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
cruise_cancel = 1
if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL] and cruise_cancel:
can_sends.append(
nissancan.create_acc_cancel_cmd(self.packer,
CS.cruise_throttle_msg, frame))
# TODO: Find better way to cancel!
# For some reason spamming the cancel button is unreliable on the Leaf
# We now cancel by making propilot think the seatbelt is unlatched,
# this generates a beep and a warning message every time you disengage
if self.CP.carFingerprint == CAR.LEAF and frame % 2 == 0:
can_sends.append(
nissancan.create_cancel_msg(self.packer, CS.cancel_msg,
cruise_cancel))
can_sends.append(
nissancan.create_steering_control(self.packer,
self.car_fingerprint,
apply_angle, frame, enabled,
self.lkas_max_torque))
if frame % 2 == 0:
can_sends.append(
nissancan.create_lkas_hud_msg(self.packer, lkas_hud_msg,
enabled, left_line, right_line,
left_lane_depart,
right_lane_depart))
if frame % 50 == 0:
can_sends.append(
nissancan.create_lkas_hud_info_msg(self.packer,
lkas_hud_info_msg,
steer_hud_alert))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart, dragonconf):
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
if not enabled or CS.steer_state in [9, 25] or abs(CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 150 and CS.CP.carFingerprint in [CAR.RAV4H, CAR.PRIUS]):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if not dragonconf.dpToyotaSng and CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
# 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.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steerAngle, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if not dragonconf.dpAtl:
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert == VisualAlert.steerRequired
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
# dp
if dragonconf.dpToyotaLdw:
dragon_left_lane_depart = left_lane_depart
dragon_right_lane_depart = right_lane_depart
else:
dragon_left_lane_depart = False
dragon_right_lane_depart = False
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_speed, pcm_override,
pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes,
dragonconf, hud_show_car, hud_alert):
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required,
CS.lkMode)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
can_sends = []
# 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
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(
self.packer, apply_steer, lkas_active, CS.CP.carFingerprint,
idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl))
# Send dashboard UI commands.
if not dragonconf.dpAtl and (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx, CS.CP.isPandaBlack,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx,
CS.CP.isPandaBlack))
if dragonconf.dpAtl:
pass
# If using stock ACC, spam cancel command to kill gas when OP disengages.
elif not dragonconf.dpAllowGas and pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
pass # TODO: implement
else:
if not dragonconf.dpAtl:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0,
P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last,
self.last_pump_ts, ts)
can_sends.append(
hondacan.create_brake_command(
self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, hud.fcw, idx,
CS.CP.carFingerprint, CS.CP.isPandaBlack,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas,
idx))
return can_sends
def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
hud_v_cruise, hud_show_lanes, dragonconf, hud_show_car,
hud_alert):
P = self.params
if enabled:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo,
CS.CP.carFingerprint)
else:
accel = 0.0
gas, brake = 0.0, 0.0
# *** apply brake hysteresis ***
pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(
brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** rate limit after the enable check ***
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2.,
DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
can_sends = []
# tester present - w/ no response (keeps radar disabled)
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
if (frame % 10) == 0:
can_sends.append(
(0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
# 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
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(
self.packer, apply_steer, lkas_active, CS.CP.carFingerprint,
idx, CS.CP.openpilotLongitudinalControl))
stopping = actuators.longControlState == LongCtrlState.stopping
starting = actuators.longControlState == LongCtrlState.starting
# wind brake from air resistance decel at high speed
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0],
[0.001, 0.002, 0.15])
# all of this is only relevant for HONDA NIDEC
max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP,
P.NIDEC_MAX_ACCEL_V)
# TODO this 1.44 is just to maintain previous behavior
pcm_speed_BP = [-wind_brake, -wind_brake * (3 / 4), 0.0, 0.5]
# The Honda ODYSSEY seems to have different PCM_ACCEL
# msgs, is it other cars too?
if CS.CP.enableGasInterceptor:
pcm_speed = 0.0
pcm_accel = int(0.0)
elif CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
pcm_speed_V = [
0.0,
clip(CS.out.vEgo - 3.0, 0.0, 100.0),
clip(CS.out.vEgo + 0.0, 0.0, 100.0),
clip(CS.out.vEgo + 5.0, 0.0, 100.0)
]
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int((1.0) * 0xc6)
else:
pcm_speed_V = [
0.0,
clip(CS.out.vEgo - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + 2.0, 0.0, 100.0),
clip(CS.out.vEgo + 5.0, 0.0, 100.0)
]
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6)
if dragonconf.dpAtl and not dragonconf.dpAtlOpLong:
pass
elif not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if not dragonconf.dpAllowGas and pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH,
CAR.INSIGHT):
rough_lead_speed = self.rough_speed(CS.lead_distance)
if CS.lead_distance > (self.stopped_lead_distance +
15.0) or rough_lead_speed > 0.1:
self.stopped_lead_distance = 0.0
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
if CS.hud_lead == 1:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
else:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx,
CS.CP.carFingerprint))
else:
self.stopped_lead_distance = CS.lead_distance
self.prev_lead_distance = CS.lead_distance
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if dragonconf.dpAtl and dragonconf.dpAtlOpLong and not CS.out.cruiseActualEnabled:
accel = 0.
gas = 0.
self.brake_last = 0.
if CS.CP.carFingerprint in HONDA_BOSCH:
accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP,
P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(
hondacan.create_acc_commands(self.packer, enabled,
active, accel, bosch_gas,
idx, stopping, starting,
CS.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(
clip(apply_brake * P.NIDEC_BRAKE_MAX, 0,
P.NIDEC_BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts,
ts)
pcm_override = True
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd,
fcw_display, idx,
CS.CP.carFingerprint,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# way too aggressive at low speed without this
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
# when you do enable.
if enabled:
apply_gas = clip(
gas_mult * (gas - brake + wind_brake * 3 / 4),
0., 1.)
else:
apply_gas = 0.0
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required,
CS.lkMode)
# Send dashboard UI commands.
if dragonconf.dpAtl and not dragonconf.dpAtlOpLong:
pass
elif (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx,
CS.CP.openpilotLongitudinalControl,
CS.stock_hud))
return can_sends
