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
lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED and CS.enable_lkas
if (frame % P.STEER_STEP) == 0:
if lkas_enabled:
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
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))
# Pedal
if CS.CP.enableGasInterceptor:
if (frame % 2) == 0:
idx = (frame // 2) % 4
zero = 0.15625 * 2 #40/256
accel = (1 -
zero) * actuators.gas + self.apply_pedal_last * zero
regen_brake = zero * actuators.brake
final_accel = accel - regen_brake
if not enabled or not CS.adaptive_Cruise:
final_accel = 0.
final_accel, self.accel_steady = accel_hysteresis(
final_accel, self.accel_steady)
final_pedal = clip(final_accel, 0., 1.)
self.apply_pedal_last = final_pedal
can_sends.append(
create_gas_command(self.packer_pt, final_pedal, idx))
# 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) - Delete
# and that ADAS is alive (10hz)
#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 = lkas_enabled == 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 in [
VisualAlert.steerRequired, VisualAlert.ldw
]
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,
hud_v_cruise, hud_show_lanes, 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)
# *** 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(pre_limit_brake, self.brake_last, -2.,
DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
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
# 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))
# 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
# Prevent rolling backwards
accel = -4.0 if stopping else accel
# 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.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_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_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6)
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
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 using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP,
P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(
hondacan.create_acc_commands(self.packer, enabled,
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.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)
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
apply_gas = clip(gas_mult * gas, 0., 1.)
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)
# Send dashboard UI commands.
if (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
def update(self, sendcan, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead, advance_time_gap = False):
# *** 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
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
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 CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.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.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if self.angle_control:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
# added to support OpenpilotButtons -- https://github.com/rhinodavid/OpenpilotButtons
# check to see if the number of time gap lines displayed by the car matches the number
# we are trying to command. if they are different, send a distance of 1 to change the displayed number of lines
advance_acc_time_gap_setting = 0
now_time = sec_since_boot()
if advance_time_gap and now_time - self.last_acc_advance_time > 0.05:
# rate limit this to once per 0.05 seconds
# was getting a soft disengage from openpilot without the rate limit
advance_acc_time_gap_setting = 1
self.last_acc_advance_time = now_time
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead, advance_acc_time_gap_setting))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, advance_acc_time_gap_setting))
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))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame/10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** 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 self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame / 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame/100)%0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** 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
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
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 CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
# dragonpilot
if enabled and (CS.left_blinker_on or CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1":
self.turning_signal_timer = 100
if self.turning_signal_timer > 0:
self.turning_signal_timer -= 1
apply_steer_req = 0
#*** 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.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if self.angle_control:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
# DragonAllowGas
# if we detect gas pedal pressed, we do not want OP to apply gas or brake
# gasPressed code from interface.py
if CS.CP.enableGasInterceptor:
# use interceptor values to disengage on pedal press
gasPressed = CS.pedal_gas > 15
else:
gasPressed = CS.pedal_gas > 0
if params.get("DragonAllowGas") == "1" and gasPressed:
apply_accel = 0
apply_gas = 0
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
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))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame//10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** 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 self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame // 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame // 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
return can_sends
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep != 0 else snd_chime
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.read_distance_lines)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024/4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
else:
STEER_MAX = 0x1000
#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)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(enabled, CS, frame, actuators)
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-alca_steer * STEER_MAX, -STEER_MAX, STEER_MAX))
if CS.cstm_btns.get_button_status("lka") == 0:
apply_steer = 0
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
lkas_active = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint, idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame/10) % 4
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx))
elif CS.stopped:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame / 2
pump_on, self.last_pump_ts = brake_pump_hysteresys(apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx))
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))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
""" Controls thread """
if not self.enable_camera:
return
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1. / 100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep is not 0 else snd_chime
# Do not send audible alert when steering is disabled or blinkers on
#if not CS.lkMode or CS.left_blinker_on or CS.right_blinker_on:
# snd_chime = 0
#print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required, CS.read_distance_lines, CS.lkMode,
self.speed_units)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 / 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
STEER_MAX = 0x7FFF
else:
STEER_MAX = 0x1000
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode and not CS.left_blinker_on and not CS.right_blinker_on # add LKAS button to toggle steering
# Send CAN commands.
can_sends = []
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint,
idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame / 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, idx))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx))
elif CS.stopped:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15,
CAR.ACCORDH, CAR.INSIGHT):
if CS.lead_distance > (self.prev_lead_distance +
float(kegman.conf['leadDistance'])):
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx))
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH):
if CS.hud_lead == 1:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx))
else:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx))
else:
self.prev_lead_distance = CS.lead_distance
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame / 2
pump_on, self.last_pump_ts = brake_pump_hysteresys(
apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.chime,
hud.fcw, idx))
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))
sendcan.send(
can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
# *** compute control surfaces ***
# gas and brake
interceptor_gas_cmd = 0.
pcm_accel_cmd = actuators.gas - actuators.brake
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.06 offset to reduce ABS pump usage when OP is engaged
interceptor_gas_cmd = clip(actuators.gas, 0., 1.)
pcm_accel_cmd = 0.06 - actuators.brake
pcm_accel_cmd, self.accel_steady = accel_hysteresis(
pcm_accel_cmd, self.accel_steady, enabled)
pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE,
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]:
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 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
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))
# 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
# Lexus IS uses a different cancellation message
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, pcm_accel_cmd,
pcm_cancel_cmd, self.standstill_req,
lead, CS.acc_type))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, CS.acc_type))
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
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))
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, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd,
hud_alert, audible_alert, forwarding_camera, left_line,
right_line, lead, left_lane_depart, right_lane_depart):
#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)
# *** 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)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None and not CS.indi_toggle:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# steer torque
alca_angle, alca_steer, alca_enabled, turn_signal_needed = self.ALCA.update(
enabled, CS, frame, actuators)
#apply_steer = int(round(alca_steer * STEER_MAX))
self.phantom.update()
# steer torque
if self.phantom.data["status"]:
apply_steer = int(round(self.phantom.data["angle"]))
if abs(CS.angle_steers) > 400:
apply_steer = 0
else:
apply_steer = int(round(alca_steer * SteerLimitParams.STEER_MAX))
if abs(CS.angle_steers) > 100:
apply_steer = 0
if not CS.lane_departure_toggle_on:
apply_steer = 0
# only cut torque when steer state is a known fault
if CS.steer_state in [3, 7, 9, 11, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
cutout_time = 100 if self.phantom.data["status"] else 200
if not enabled or (frame - self.last_fault_frame < cutout_time):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
apply_steer = apply_toyota_steer_torque_limits(apply_steer,
self.last_steer,
CS.steer_torque_motor,
SteerLimitParams)
if apply_steer == 0 and self.last_steer == 0:
apply_steer_req = 0
if not enabled and right_lane_depart and CS.v_ego > 12.5 and not CS.right_blinker_on:
apply_steer = self.last_steer + 3
apply_steer = min(apply_steer, 800)
#print "right"
#print apply_steer
apply_steer_req = 1
if not enabled and left_lane_depart and CS.v_ego > 12.5 and not CS.left_blinker_on:
apply_steer = self.last_steer - 3
apply_steer = max(apply_steer, -800)
#print "left"
#print apply_steer
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = alca_angle
#apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(
self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP,
ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim,
self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
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 CS.standstill and not self.last_standstill:
# self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.standstill
can_sends = []
# Enable blindspot debug mode once
if BLINDSPOTDEBUG:
self.blindspot_poll_counter += 1
if self.blindspot_poll_counter > 1000: # 10 seconds after start
if CS.left_blinker_on:
self.blindspot_blink_counter_left += 1
#print "debug Left Blinker on"
elif CS.right_blinker_on:
self.blindspot_blink_counter_right += 1
else:
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
#print "debug Left Blinker off"
if self.blindspot_debug_enabled_left:
can_sends.append(
set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
self.blindspot_debug_enabled_left = False
#print "debug Left blindspot debug disabled"
if self.blindspot_debug_enabled_right:
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self.blindspot_debug_enabled_right = False
#print "debug Right blindspot debug disabled"
if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds
can_sends.append(set_blindspot_debug_mode(
LEFT_BLINDSPOT, True))
#print "debug Left blindspot debug enabled"
self.blindspot_debug_enabled_left = True
if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode
if CS.v_ego > 6: #polling at low speeds switches camera off
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
#print "debug Right blindspot debug enabled"
self.blindspot_debug_enabled_right = True
if self.blindspot_debug_enabled_left:
if self.blindspot_poll_counter % 20 == 0 and self.blindspot_poll_counter > 1001: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
if self.blindspot_debug_enabled_right:
if self.blindspot_poll_counter % 20 == 10 and self.blindspot_poll_counter > 1005: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
#*** 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.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(
create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(
create_steer_command(self.packer, apply_steer,
apply_steer_req, frame))
if self.angle_control:
can_sends.append(
create_ipas_steer_command(self.packer, apply_angle,
self.steer_angle_enabled, ECU.APGS
in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0,
True))
if CS.cstm_btns.get_button_status("tr") > 0:
distance = 1
else:
distance = 0
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (
pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
can_sends.append(
create_accel_command(self.packer, apply_accel,
pcm_cancel_cmd, self.standstill_req,
lead, distance))
else:
can_sends.append(
create_accel_command(self.packer, 0, pcm_cancel_cmd, False,
lead, distance))
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))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame // 10, addr))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert, audible_alert)
steer, fcw, sound1, sound2 = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(
create_ui_command(self.packer, steer, sound1, sound2,
left_line, right_line, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSSP2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** 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 self.car_fingerprint in cars and not (
ecu == ECU.CAM and forwarding_camera):
# special cases
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
cnt = (((frame // 5) % 7) + 1) << 5
vl = chr(cnt) + vl
elif addr in (0x489, 0x48a) and bus == 0:
# add counter for those 2 messages (last 4 bits)
cnt = ((frame // 100) % 0xf) + 1
if addr == 0x48a:
# 0x48a has a 8 preceding the counter
cnt += 1 << 7
vl += chr(cnt)
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
def update(self, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
# *** apply brake hysteresis ***
#brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = True
# *** rate limit after the enable check ***
#self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes or 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
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep != 0 else snd_chime
# Do not send audible alert when steering is disabled or blinkers on
#if not CS.lkMode or CS.left_blinker_on or CS.right_blinker_on:
# snd_chime = 0
#print("{0} {1} {2}".format(chime, alert_id, hud_alert))
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel),
int(round(hud_v_cruise)), 1, hud_car, 0xc1, hud_lanes,
int(snd_beep), snd_chime, fcw_display, acc_alert,
steer_required, CS.lkMode)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 // 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
STEER_MAX = 0x7FFF
else:
STEER_MAX = 0x1000
if self.BP2 is None:
self.BP2 = int(kegman.conf['BP2']) if int(
kegman.conf['BP2']) != 0 else STEER_MAX
self.BP1 = int(
kegman.conf['BP1']) if int(kegman.conf['BP1']) != 0 else 2560
print("torque breakpoints set!", self.BP1, self.BP2)
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -self.BP2, self.BP2))
apply_steer = int(
interp(apply_steer, [-self.BP2, -self.BP1, self.BP1, self.BP2],
[-STEER_MAX, -self.BP1, self.BP1, STEER_MAX]))
lkas_active = not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
can_sends = []
# 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))
can_sends.append(
hondacan.create_steering_control_x2(self.packer,
CS.CP.carFingerprint, idx,
CS.CP.isPandaBlack))
# Send dashboard UI commands.
if (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))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.standstill and enabled and CS.stopped and CS.lead_distance < 200:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15,
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, CS.CP.isPandaBlack))
print("spamming")
print(self.stopped_lead_distance, CS.lead_distance,
rough_lead_speed)
elif False and CS.CP.carFingerprint in (CAR.CIVIC_BOSCH,
CAR.CRV_HYBRID):
print(' this is a civic or crv')
if CS.hud_lead == 1:
can_sends.append(
hondacan.spam_buttons_command(
self.packer, CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint, CS.CP.isPandaBlack))
else:
print(' doing the regular SPAM RESUME')
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL,
idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.v_ego == 0:
#print(' not quite "stopped" yet, but lead distance is %f' % CS.lead_distance)
self.stopped_lead_distance = CS.lead_distance
self.prev_lead_distance = CS.lead_distance
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
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.chime,
hud.fcw, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
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, 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, 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_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
# *** compute control surfaces ***
self.sm.update(0)
if self.sm.updated['radarState']:
self.lead_v = self.sm['radarState'].leadOne.vRel
self.lead_a = self.sm['radarState'].leadOne.aRel
self.lead_d = self.sm['radarState'].leadOne.dRel
if self.sm.updated['controlsState']:
self.LCS = self.sm['controlsState'].longControlState
if frame % 500 == 0: # *** 5 sec interval? ***
print(self.LCS)
# gas and brake
# apply_gas = 0.
# apply_accel = actuators.gas - actuators.brake
# if CS.CP.enableGasInterceptor and enabled and CS.out.vEgo < MIN_ACC_SPEED:
# apply_gas = clip(actuators.gas, 0., 1.)
# # converts desired acceleration to gas percentage for pedal
# # +0.06 offset to reduce ABS pump usage when applying very small gas
# if apply_accel * CarControllerParams.ACCEL_SCALE > coast_accel(CS.out.vEgo):
# apply_gas = clip(compute_gb_pedal(apply_accel * CarControllerParams.ACCEL_SCALE, CS.out.vEgo), 0., 1.)
# apply_accel += 0.06
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 # Original
if lead:
apply_accel = 0.06 - actuators.brake
#apply_accel = 0.06
else:
apply_accel = 0.06
# End new
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 * CarControllerParams.ACCEL_SCALE,
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 abs(CS.out.steeringRateDeg) > 100:
#if not enabled or CS.steer_state in [9, 25] or CS.out.epsDisabled==1 or abs(CS.out.steeringRateDeg) > 100: #Original statement
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
#if not enabled and CS.pcm_acc_status: # Original
if not 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 = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR and not self.standstill_hack:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
can_sends = []
if (frame % 2 == 0):
can_sends.append(
create_lead_command(self.packer, self.lead_v, self.lead_a,
self.lead_d))
#*** 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))
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))
# 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 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)) # Original value
can_sends.append(
create_accel_command(self.packer, 500, 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
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, left_lane_depart,
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
# 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, 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
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** 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
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
#print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
apply_angle = actuators.steerAngle
angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V)
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
else:
apply_angle = CS.angle_steers
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 CS.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_angle = apply_angle
self.last_accel = apply_accel
self.last_standstill = CS.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.CAM in self.fake_ecus:
if self.angle_control:
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
else:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
if self.angle_control:
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
ECU.APGS in self.fake_ecus))
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
if CS.cstm_btns_tr > 0:
distance = 1
else:
distance = 0
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead, distance))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, distance))
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
alert_out = process_hud_alert(hud_alert)
steer, fcw = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** 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 self.car_fingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus, False))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
forwarding_camera, left_line, right_line, lead,
left_lane_depart, right_lane_depart):
can_sends = []
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = clip(actuators.brake, 0., 1.)
# steer torque
apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
#only cut torque when steer state is a known fault
if (CS.steer_state >= 1):
self.last_fault_frame = frame
#Cut steering for 0.5s after fault
if not enabled or (frame - self.last_fault_frame < 10):
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
self.last_steer = apply_steer
self.last_standstill = CS.standstill
can_sends = []
if (frame % 2 == 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
can_sends.append(
create_gas_command(self.packer, apply_gas, frame // 2))
can_sends.append(
create_brake_command(self.packer, apply_brake, frame // 2))
# steering at 100hz
can_sends.append(
create_steer_command(self.packer, apply_steer, apply_steer_req,
frame))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
alert_out = process_hud_alert(hud_alert)
steer, fcw = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0:
can_sends.append(make_toyota_can_msg(addr, vl, bus, False))
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, hud_show_car, hud_alert):
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.v_ego,
CS.CP.carFingerprint)
# *** no output if not enabled ***
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 = 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:
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)
# **** process the car messages ****
# *** compute control surfaces ***
BRAKE_MAX = 1024 // 4
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
STEER_MAX = 0x7FFF
elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
STEER_MAX = 0x1400
else:
STEER_MAX = 0x1000
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(
clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
if apply_steer > 0xA00:
apply_steer = (apply_steer - 0xA00) / 2 + 0xA00
elif apply_steer < -0xA00:
apply_steer = (apply_steer + 0xA00) / 2 - 0xA00
lkas_active = enabled and not CS.steer_not_allowed
# Send CAN commands.
can_sends = []
# 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))
# Send dashboard UI commands.
if (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.stock_hud))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.stopped:
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
# pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
pump_on, self.last_pump_on_state = brake_pump_hysteresis(
apply_brake, self.apply_brake_last,
self.last_pump_on_state, ts)
if CS.CP.enableGasInterceptor:
pcm_cancel_cmd = False
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, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart,
right_lane_depart):
#if not enabled:
# self.sm.update(0)
#if self.sm.updated['pathPlan']:
# blinker = CS.out.leftBlinker or CS.out.rightBlinker
# ldw_allowed = CS.out.vEgo > 12.5 and not blinker
# CAMERA_OFFSET = op_params.get('camera_offset', 0.06)
# right_lane_visible = self.sm['pathPlan'].rProb > 0.5
# left_lane_visible = self.sm['pathPlan'].lProb > 0.5
# self.rightLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].rPoly[3] > -(0.93 + CAMERA_OFFSET) and right_lane_visible)
# self.leftLaneDepart = bool(ldw_allowed and self.sm['pathPlan'].lPoly[3] < (0.93 - CAMERA_OFFSET) and left_lane_visible)
# print("blinker")
# print(blinker)
# print("ldw_allowed")
# print(ldw_allowed)
# print("CAMERA_OFFSET")
# print(CAMERA_OFFSET)
# print("right_lane_visible")
# print(right_lane_visible)
# print("left_lane_visible")
# print(left_lane_visible)
# print("self.rightLaneDepart")
# print(self.rightLaneDepart)
# print("self.leftLaneDepart")
# print(self.leftLaneDepart)
# *** 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)
if CS.CP.enableGasInterceptor:
if CS.out.gasPressed:
apply_accel = max(apply_accel, 0.06)
if CS.out.brakePressed:
apply_gas = 0.0
apply_accel = min(apply_accel, 0.00)
else:
if CS.out.gasPressed:
apply_accel = max(apply_accel, 0.0)
if CS.out.brakePressed and CS.out.vEgo > 1:
apply_accel = min(apply_accel, 0.0)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 1s after fault
if (frame - self.last_fault_frame < 100) or abs(
CS.out.steeringRate) > 100 or (abs(CS.out.steeringAngle) > 100
and CS.CP.carFingerprint
in [CAR.RAV4H, CAR.PRIUS]):
new_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and right_lane_depart and CS.out.vEgo > 12.5 and not CS.out.rightBlinker:
new_steer = self.last_steer + 3
new_steer = min(new_steer, 800)
print("right")
print(new_steer)
apply_steer_req = 1
if not enabled and left_lane_depart and CS.out.vEgo > 12.5 and not CS.out.leftBlinker:
new_steer = self.last_steer - 3
new_steer = max(new_steer, -800)
print("left")
print(new_steer)
apply_steer_req = 1
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.last_steer, CS.out.steeringTorqueEps,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
if not enabled and abs(apply_steer) > 800 and not (right_lane_depart or
left_lane_depart):
apply_steer = 0
apply_steer_req = 0
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 CS.out.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
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))
# 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 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
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, left_lane_depart,
right_lane_depart))
if frame % 100 == 0 and (Ecu.dsu in self.fake_ecus
or Ecu.unknown 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))
# Enable blindspot debug mode once
if frame > 1000 and not (
CS.CP.carFingerprint in TSS2_CAR or CS.CP.carFingerprint
== CAR.CAMRY): # 10 seconds after start and not a tss2 car
if BLINDSPOTALWAYSON:
self.blindspot_blink_counter_left += 1
self.blindspot_blink_counter_right += 1
#print("debug blindspot alwayson!")
elif CS.out.leftBlinker:
self.blindspot_blink_counter_left += 1
#print("debug Left Blinker on")
elif CS.out.rightBlinker:
self.blindspot_blink_counter_right += 1
#print("debug Right Blinker on")
else:
self.blindspot_blink_counter_left = 0
self.blindspot_blink_counter_right = 0
if self.blindspot_debug_enabled_left:
can_sends.append(
set_blindspot_debug_mode(LEFT_BLINDSPOT, False))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x01\x00\x00\x00\x00', 0))
self.blindspot_debug_enabled_left = False
#print ("debug Left blindspot debug disabled")
if self.blindspot_debug_enabled_right:
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0))
self.blindspot_debug_enabled_right = False
#print("debug Right blindspot debug disabled")
if self.blindspot_blink_counter_left > 9 and not self.blindspot_debug_enabled_left: #check blinds
can_sends.append(set_blindspot_debug_mode(
LEFT_BLINDSPOT, True))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x10\x60\x00\x00\x00\x00', 0))
#print("debug Left blindspot debug enabled")
self.blindspot_debug_enabled_left = True
if self.blindspot_blink_counter_right > 5 and not self.blindspot_debug_enabled_right: #enable blindspot debug mode
if CS.out.vEgo > 6: #polling at low speeds switches camera off
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x60\x00\x00\x00\x00', 0))
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, True))
#print("debug Right blindspot debug enabled")
self.blindspot_debug_enabled_right = True
if CS.out.vEgo < 6 and self.blindspot_debug_enabled_right: # if enabled and speed falls below 6m/s
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x10\x01\x00\x00\x00\x00', 0))
can_sends.append(
set_blindspot_debug_mode(RIGHT_BLINDSPOT, False))
self.blindspot_debug_enabled_right = False
#print("debug Right blindspot debug disabled")
if self.blindspot_debug_enabled_left:
if frame % 20 == 0 and frame > 1001: # Poll blindspots at 5 Hz
can_sends.append(poll_blindspot_status(LEFT_BLINDSPOT))
#can_sends.append(make_can_msg(0x750, b'\x41\x02\x21\x69\x00\x00\x00\x00', 0))
#print("debug Left blindspot poll")
if self.blindspot_debug_enabled_right:
if frame % 20 == 10 and frame > 1005: # Poll blindspots at 5 Hz
#can_sends.append(make_can_msg(0x750, b'\x42\x02\x21\x69\x00\x00\x00\x00', 0))
can_sends.append(poll_blindspot_status(RIGHT_BLINDSPOT))
#print("debug Right blindspot poll")
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):
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True
self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False
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
# *** 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
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# # Cut steering for 2s after fault
if not enabled: # or (frame - self.last_fault_frame < 200):
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 self.dragon_toyota_sng_mod and CS.out.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
# dp
apply_steer_req = common_controller_ctrl(enabled,
self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal,
CS.out.leftBlinker,
CS.out.rightBlinker,
apply_steer_req)
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))
# 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 pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
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 self.dragon_lane_departure_warning:
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, 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.read_distance_lines, CS.lkMode)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
# return minimum of brake_last*MAX, or MAX-1, but not less than zero
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
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 #and not CS.left_blinker_on and not CS.right_blinker_on # add LKAS button to toggle steering
# Send CAN commands.
can_sends = []
# 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))
# Send dashboard UI commands.
if (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.stock_hud))
if CS.CP.radarOffCan:
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
'''
elif CS.stopped:
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT):
if CS.lead_distance > (self.prev_lead_distance + float(kegman.conf['leadDistance'])):
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH):
if CS.hud_lead == 1:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
else:
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
else:
self.prev_lead_distance = CS.lead_distance
'''
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
pump_on, self.last_pump_on_state = brake_pump_hysteresis(
apply_brake, self.apply_brake_last,
self.last_pump_on_state, ts)
# Do NOT send the cancel command if we are using the pedal. Sending cancel causes the car firmware to
# turn the brake pump off, and we don't want that. Stock ACC does not send the cancel cmd when it is braking.
if CS.CP.enableGasInterceptor:
pcm_cancel_cmd = False
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, CS, frame, actuators, pcm_speed, pcm_override,
pcm_cancel_cmd, pcm_accel, hud_v_cruise, hud_show_lanes,
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:
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)
# **** 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
# Send CAN commands.
can_sends = []
# 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 (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 using stock ACC, spam cancel command to kill gas when OP disengages.
if 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:
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, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# *** 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)
# Get the angle from ALCA.
alca_enabled = False
alca_steer = 0.
alca_angle = 0.
turn_signal_needed = 0
# Update ALCA status and custom button every 0.1 sec.
if self.ALCA.pid == None:
self.ALCA.set_pid(CS)
if (frame % 10 == 0):
self.ALCA.update_status(CS.cstm_btns.get_button_status("alca") > 0)
# 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
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# Cut steering for 2s after fault
if not enabled or (frame - self.last_fault_frame < 200):
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 CS.out.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
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))
# 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 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
alert_out = process_hud_alert(hud_alert)
steer, fcw = alert_out
if (any(alert_out) and not self.alert_active) or \
(not any(alert_out) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
else:
send_ui = False
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** 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 self.car_fingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
